Oberseminar 2014 TAMS activities in RobotEra Hannes Bistry - - PowerPoint PPT Presentation

University of Hamburg

MIN Faculty Department of Informatics

• 3. June 2014

Oberseminar 2014

TAMS activities in RobotEra Hannes Bistry

University of Hamburg Faculty of Mathematics, Informatics and Natural Sciences Department of Informatics Technical Aspects of Multimodal Systems

3.June 2014

TAMS 1

University of Hamburg

MIN Faculty Department of Informatics

• 3. June 2014

Outline

• 1. Introduction
• 2. ROS Overview
• 3. Example of a ROS-based robot
• 4. Camera Integration
• 5. MoveIt Integration

TAMS 2

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

General problem of research in robotics

(from willowgarage.com)

TAMS 3

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS

Robot Operating System

◮ component-oriented robotics framework ◮ Inter Process Communication middleware ◮ development suite ◮ language support (C++, Python, Java,...) ◮ debugging functions ◮ package management system ◮ active community

TAMS 4

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - basic concepts

Main components:

◮ roscore (directory service and parameter storage)

◮ usually one in an environment

◮ Nodes (each component is called a node) ◮ Communication mechanisms

◮ topics ◮ services ◮ (actions)

◮ Parameter Server

◮ part of the “roscore“ ◮ configure nodes / share files and information

TAMS 5

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Topics

Each Node can listen or publish on arbitrary topics:

◮ a “named“ datastream of a certain type (e.g.

“/front camera/image raw“)

◮ N:N communication ◮ every node can listen to a topic

◮ example: multiple nodes need data from the camera

◮ multiple nodes can publish on a topic

◮ example: all nodes may generate debug output (collected)

◮ Problem: dysfunctional nodes may break the whole system

◮ trusted zone ◮ ROS offers good debugging options

The topic names are used to match clients and servers.

TAMS 6

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Topics

Every topic is restricted to one Message type:

◮ (first come - first serve to announce the message type for a topic) ◮ Built-in types string, float (32 or 64 bits), integer, booleans and

Header

◮ defined in other own and third-party packages

“MyOtherPackage/CustomMessage“ Definition of new message formats in a .msg file: float64 myDouble string myString float64 [ ] myArrayOfDouble The message definition can also contain full messages defined in other packages (nested definition).

TAMS 7

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Topics - Properties

◮ “Fire-and-Forget“ style of communication ◮ comparable to UDP-packets ◮ messages may be dropped due to:

◮ limited network bandwidth ◮ processing of last message lasted too long

Behavior can be adjusted:

◮ message queue length ◮ whether to latch last message for new subscribers

TAMS 8

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Topics - Examples

◮ joint values ◮ camera images ◮ laser scanner data ◮ point cloud (Microsoft Kinect) ◮ system health status (temperature, battery, ...)

TAMS 9

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Topics - Examples

Publishing from the command line:

rostopic pub /topic_name std_msgs/String ” Hello World”

Receiving the Topic:

rostopic echo /topic_name data : Hello World − − −

TAMS 10

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Topics - Example N:N communication

Publishing repeatedly from the command line 1: rostopic pub −r 1 /topic_name std_msgs/String ” Hello ” Publishing repeatedly from the command line 2: rostopic pub −r 1 /topic_name std_msgs/String ”World” Receiving the Topic: rostopic echo /topic_name data : Hello − − − data : World − − − data : Hello − − − data : World

TAMS 11

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Topics - Invalid message types

Publishing repeatedly from the command line 1: rostopic pub −r 1 /topic_name std_msgs/String ” H e l l o ” Listen to message with command line 2: rostopic echo / topic_name Publishing repeatedly from the command line 3: rostopic pub −r 1 /topic_name std_msgs/Int16 42 Error Message: [ WARN ] [ WallTime : 1403186454.670754] Could not process inbound connection : topic types do not match : [ std_msgs/String ] vs . [ std_msgs/Int16 ]{ ’ message_definition ’ : ’ string data\n\n ’ , ’ callerid ’ : ’/ rostopic_12368_1403186454023 ’ , ’ tcp_nodelay ’ : ’0 ’ , ’ md5sum ’ : ’992 ce8a1687cec8c8bd883ec73ca41d1 ’ , ’ topic ’ : ’/ topic_name ’ , ’ type ’ : ’ std_msgs/ String ’}

TAMS 12

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Services

Nodes can provide and call so called Services.

◮ asynchronous communication (RPC) ◮ Service request / reply ◮ each part has its of datatype

◮ definition similar to messages in a .srv file (request / reply are

separated by dashes) float64 query_x float64 query_y − − − float64 result

TAMS 13

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Actions

Interface for preemptable tasks.

◮ for longer lasting tasks ◮ goal / result /feedback

◮ client calls service with goal message ◮ (server sends one/many feedback messages) ◮ server sends result ◮ definition similar to messages in a .action file (example from

ros.org)

# Define the goal uint32 dishwasher_id # S p e c i f y which dishwasher we want to use − − − # Define the r e s u l t uint32 total_dishes_cleaned − − − # Define a feedback message float32 percent_complete

TAMS 14

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Actions

TAMS 15

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Filesystem Structure

ROS workspace:

◮ the ”root”directory where all your packages are in ◮ announce this to the ROS system by

export ROS PACKAGE PATH=$ROS PACKAGE PATH:/path/to/your/workspace

ROS package:

◮ directories under one directory defined in your

ROS PACKAGE PATH variable

◮ may contain executables, libraries, configuration files, robot

models, message/service/actions definition files

◮ anything that logically constitutes a useful entity can be part of

the package

◮ may depend on other packages ◮ description / dependencies defined in Manifest.xml

TAMS 16

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

ROS - Filesystem Structure

ROS Nodes:

◮ are usually executables within one package ◮ instead of “path/to/your/workspace/package name/bin/node name“

you can simply type “rosrun package name node name“ in ANY directory Launch-files:

◮ comparable to shell-scripts ◮ .launch files in the

“path/to/your/workspace/package name/launch“ folder

◮ start roscore if no one is running ◮ start one or multiple nodes, also from other packages ◮ define parameters ◮ start with “roslaunch package name launchfile name.launch“

TAMS 17

University of Hamburg

MIN Faculty Department of Informatics 2 ROS Overview

• 3. June 2014

Gazebo Simulator

ROS Nodes:

◮ Physical simulation of robots ◮ ROS integration ◮ simulate sensors

◮ laser ◮ cameras ◮ Kinect

◮ simulate actuators

◮ robot arms ◮ drive system

Note: RViz can only visualize, not simulate.

TAMS 18

University of Hamburg

MIN Faculty Department of Informatics 3 Example of a ROS-based robot

• 3. June 2014

System Architecture of DORO

◮ Scitos G5 platform ◮ ROS Hydro (Robot Operating System) ◮ Core i7 Industrial PC ◮ Software Subsystems:

◮ MoveIt (IK, Manipulation) ◮ Mira / Cognidrive (navigation) ◮ PEIS (high level task planning) ◮ GStreamer / OpenCV for vision

TAMS 19

University of Hamburg

MIN Faculty Department of Informatics 3 Example of a ROS-based robot

• 3. June 2014

The Robot Description File

One essential part of running a Robot under ROS is a URDF description of a Robot, that is needed for:

◮ specification of coordinate frames ◮ calculation of frame transformations ◮ visualization ◮ simulation

◮ needs additional files

Usage:

◮ The file is uploaded to the ROS parameter server. ◮ several ROS-nodes fetch the robot-description file from there ◮ In practice, the URDF file is often generated by a .xacro file,

that offers comfortable features like macros.

TAMS 20

University of Hamburg

MIN Faculty Department of Informatics 3 Example of a ROS-based robot

• 3. June 2014

The Robot Description File

...

TAMS 21

University of Hamburg

MIN Faculty Department of Informatics 3 Example of a ROS-based robot

• 3. June 2014

The Robot Description File - Xacro

In practice, the URDF file is often generated by a .xacro file, that has several advantage over pure URDF

◮ define macros (i.e. sensor or actuator type) ◮ algebraic expressions ◮ define constants ◮ run ROS-commands

Example: usage of a macro:

<xacro : include filename=”$ ( f i n d d o r o d e s c r i p t i o n ) / u r d f / x t i o n . u r d f . xacro ” /> . . . <xacro : xtion parent=”${parent} l i n k ” > <origin xyz=”${0.045} 0.035 $ {0.2101 − 0.03912 + 0.02} ” rpy=”0 0 0” />

TAMS 22

University of Hamburg

MIN Faculty Department of Informatics 3 Example of a ROS-based robot

• 3. June 2014

The Robot State Publisher

The “robot state publisher“ is a ROS node, that reads out the state of all joints, and publishes frame transformations:

◮ subscribes to ROS-topic “/joint states“ and publishes “/tf“

TAMS 23

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

Implementation of Camera Systems

ROS GStreamer interface:

◮ using camera with GStreamer drivers in ROS ◮ use features of GStreamer (compression, network data transfer) ◮ existing ROS-GStreamer interface “gscam“ ◮ implemented as a standalone ROS-Node ◮ GStreamer Pipeline is configures in the environment variable

“GSCAM CONFIG“

TAMS 24

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

Drawbacks of the current integration:

◮ restricted to RGB, 24 bpp format ◮ fixated to ROS-topic (needs remapping) ◮ inefficient due to unnecessary copy operations ◮ no timestamps exchanged ◮ supports only one direction: GStreamer → ROS ◮ run-time control of parameters not possible

Thus the ROS-GStreamer integration was redesigned from the scratch.

TAMS 25

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

New ROS Integration - Concept

TAMS 26

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

ROSSink

Publish arbitrary GStreamer video streams in ROS.

gst−launch videotestsrc ! capsfilter caps=” video /x− raw−rgb , bpp=24” ! rossink topic=testvideo

Will generate the following topics:

/testvideo/ camera_info /testvideo/image_raw /testvideo/image_raw/ compressed /testvideo/image_raw/ compressed/ parameter_descriptions /testvideo/image_raw/ compressed/ parameter_updates /testvideo/image_raw/ compressedDepth /testvideo/image_raw/ compressedDepth / parameter_descriptions /testvideo/image_raw/ compressedDepth / parameter_updates /testvideo/image_raw/theora /testvideo/image_raw/theora/ parameter_descriptions /testvideo/image_raw/theora/ parameter_updates

TAMS 27

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

ROSSink (cont.)

Use cases:

◮ use arbitrary GStreamer compliant camera in ROS ◮ integration of smart camera ◮ publish preprocessed video ◮ use camera on remote system, using encoders from GStreamer

(h264) Format support: RGB, YUV,gray,jpeg Timestamp and metadata conversion.

TAMS 28

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

ROSSrc

Subscribe to a ROS Topic and “publish“ images inside GStreamer:

gst−launch rossrc topic=/testvideo/image_raw ! ffmpegcolorspace ! ximagesink sync=0

Use cases:

◮ use ROS compliant cameras in GStreamer (including virtual

cameras)

◮ record ROS image topics (with compression) ◮ drop-in replacement for Image View (ROS)

◮ Display VGA 60 Hz on Core i5: ◮ GStreamer: 19 % CPU-load ◮ Image View: 57 % CPU-load

TAMS 29

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

Sift based Object detection

TAMS 30

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

ROSSiftfolder

Implementation of SIFT-based object detection

◮ algorithm can determine the 6D pose of an object based on a

2D camera image

◮ previously known:

◮ camera calibration ◮ dimensions of the object ◮ sample image from the object

◮ algorithm detects corresponding points ◮ calculates camera position to the object ◮ currently cuboids with sufficient optical features are supported

TAMS 31

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

ROSSiftfolder

SIFT-based Object detection, publishes transformations in ROS

◮ all image files in a folder are detected ◮ Input: feature-vector ◮ currently, one dimension is encoded in the filename (like

box100.jpg )

◮ planned feature: database access

◮ if > 4 matches are found, the 3D pose is calculated

Published topics:

◮ ROS Vizualization Markers ◮ tf-Messages from camera frame to object frame

TAMS 32

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

Example: Displaying Objects with RViz

TAMS 33

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

Application: ROS-GStreamer Interface

Application: Visualization of grasp planning in a Gazebo Demo

◮ virtual camera in Gazebo publishing images in ROS ◮ virtual screen displaying images send to another ROS topic ◮ one GStreamer Pipeline, featuring an input switcher with 3

ports:

◮ one sample video ◮ the simulated Gazebo camera via the GstROSSrc ◮ the RViz window via the screen capturing plugin of GStreamer

◮ the resulting video is sent to the virtual screen in Gazebo

Currently only a demo, but can be used for next generation user interfaces.

TAMS 34

University of Hamburg

MIN Faculty Department of Informatics 4 Camera Integration

• 3. June 2014

Application: ROS-GStreamer Interface

TAMS 35

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

ROS-MoveIt

◮ software for mobile manipulation ◮ inverse kinematics implementation (IK) ◮ integration of OMPL (Open Motion Planning Library)

◮ collection of randomized motion planners

◮ collision checking library (FCL) ◮ integration of sensors

◮ robot state ◮ obstacles (any kind of Point Cloud generating sensor)

◮ World Geometry Monitor

TAMS 36

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

MoveIt Concept

Simplified function of MoveIt trajectory generation.

TAMS 37

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

MoveIt Integration: Stumbling Blocks:

◮ incompatible meshes for robot-model:

◮ self collisions were not prevented for parts of the robot ◮ solution: import and export files in MeshLab

◮ MoveIt reports self-collisions for adjacent links:

◮ motion planning fails as collision checks are positive ◮ add adjacent links to whitelist

◮ Malfunction on Intel GPUs:

◮ Robot Model is displayed incorrectly, body parts have wrong

pose, collision checks are positive, trajectory generation fails

◮ (needs testing: updated drivers from the xorg-edgers repository) ◮ stick to NVidia GPUs

TAMS 38

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

MoveIt Integration: Stumbling Blocks (cont.):

◮ wrong order of joints in trajectory message:

◮ robot arm executes wrong trajectory ◮ MoveIt ignores user-provided joint sequence ◮ Jaco-Node ignored actual sequence in the message ◮ solution: Jaco-Node fixed

◮ MoveIt chooses arbitrary arm-configuration for goal pose

◮ long trajectories with high risk of collisions ◮ examples: ◮ unnecessary rotation of first joint (shoulder yaw) by 180 degrees ◮ change from elbow up to elbow down configuration ◮ due to wide range of Jaco joints ◮ (intermediate) solution: limited range of joints in the URDF files ◮ side-effect: planning time decreased from 5 sec to 0.5 sec

TAMS 39

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Navigation

Task: we want to drive the robot to a certain point in the map.

◮ Input:

◮ goal position ◮ map ◮ angular velocity of the wheels ◮ robot geometry (URDF) ◮ sensors (laser range finders, point cloud)

◮ Output:

◮ geometry msgs/Twist to topic /cmd vel (desired velocity /

angular velocity of the robot base)

◮ feedback of action server

TAMS 40

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Navigation (cont.)

TAMS 41

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Navigation

Alternative implementation on the service robot Doro:

◮ Scitos G5 is using a different robot framework (Mira) ◮ Mira-ROS interface ◮ comercial navigation software Cognidrive ◮ exchange/translate certain topics / messages between

frameworks This is mostly a “political“ decision. (EU project partner is responsible for navigation) According to the partner, it has advantages over the ROS AMCL based navigation stack:

◮ reliability ◮ industrial proven ◮ support for precise docking

TAMS 42

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Diagnostic Aggregator - Motivation

Problem: A robot system has MANY potential points of failure:

◮ a distributed system, i.e. multiple networked PCs that may be

◮ crashed ◮ unavailable due to network conditions ◮ forgotten to switch on ◮ not properly working due to several reasons (temperature,

CPU-load, disk space, etc.)

◮ many peripheral devices, that may..

◮ be disconnected or have no power ◮ cable may be broken / loose (mechanical stress) ◮ be broken itself ◮ have driver problems (that in practise sometimes need a reboot)

TAMS 43

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Diagnostic Aggregator - Motivation (cont)

Problem: A robot system has MANY potential points of failure (cont):

◮ need multiple software components (in ROS nodes), that

◮ may be of questionable quality ◮ and therefore crashed or show other malfunctions ◮ have not been started in the right order and exited ◮ have exited due to malfunctions in other components

◮ the users/ developers from other labs, that...

◮ may provide input that no developer thinks of... ◮ close software components ◮ modify the startup scripts

How to debug this?

TAMS 44

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Diagnostic Aggregator

A ROS node collection and analysing all messages on the “/diagnostics“ topic.

◮ every Node can report problems on this topic ◮ every Node can report periodic status messages ◮ Diagnostic aggregator is configured via a .yaml file (check for

several conditions)

◮ configurable timeout fot each condition (watchdog like

• peration)

◮ conditions may be grouped ◮ additional aggregators may be implemented

TAMS 45

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Diagnostic Aggregator

The result of the Aggregator is published on the “/diagnostics agg“ topic and may be displayed with a GUI.

TAMS 46

University of Hamburg

MIN Faculty Department of Informatics 5 MoveIt Integration

• 3. June 2014

Software Architecture of DORO

handover

• bject-

services supervisor and sequencer

BringObjectService GraspObjectService MoveToService CameraImageService

OMPL OpenCV

Kinova.DLL gstreamer OpenNI

Orocos

rospy roscore

ROS

MIRA-ROS

PCL SCITOS

PEIS . . .

rviz Center MIRA-

MIRA / CogniDrive

Cameras Kinect

laser- scanners

Jaco MoveIt!

roscpp

navigation_stack

• bject_perception

environment_perception

messages doro-GUI

HRI

TAMS 47