Control del movimiento de un robot

usando reconocimiento de carasProyecto de la materia ”Temas Especıficos de Control I (vision)”

Jonas Meier

21 de noviembre de 2014

Indice general

1. La tarea 21.1. Objetivo del trabajo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2. Especificaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3. Tareas a realizar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

2. El robot 32.1. Hardware . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.2. Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

3. Solucion y Implementacion 53.1. Procesamiento del imagen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53.2. Control de los motores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73.3. Resultado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

1

Capıtulo 1

La tarea

1.1. Objetivo del trabajo

El robot se orientara siguiendo a la persona mas cercana. Para realizar esta tarea, reconoce carasy rota en ese direccion.

1.2. Especificaciones

Realizar el seguimiento de personas por un robot mediante el uso de una camara kinect y unsistema de control ad hoc.

1.3. Tareas a realizar

leer la camara kinect del robot

usar un filtro cascade de Haar para reconocer caras

amortiguar errores mediante un filtro Kalman

calcular la posicion de la cara relativa al robot

2

Capıtulo 2

El robot

2.1. Hardware

El robot, que uso, se llama Turtlebot. Es un robot barato, con open-source-software. Esta confor-mado de:

un Kobuki-Base, los motores del robot

una camara Kinect para optener imagenes de RGB y 3D

una netbook para el sistema operativo del robot

Figura 2.1: El robot

2.2. Software

Turtlebot funciona utilizando el Software ROS (Robot Operating System). Es un framework pro-gramado en C/C++ y Python. El sistema se basa en una estructura de nodos, cada nodo puedecomunicarse con otros nodos usando el ROS-message-system. Existen nodos para comunicarse con elhardware del turtlebot. Hay un nodo asociado a la camara que publica todas las imagenes de la cameraKinect y otro que recibe instrucciones para los motores.

3

Figura 2.2: Los nodos del robot

4

Capıtulo 3

Solucion y Implementacion

Para realizar el objetivo del trabajo lo divido en dos partes:

procesamiento de la imagen

comunicacion con los motores

El procesamiento toma las imagenes de la camara Kinect y calcula la posicion de la persona mascercana. Luego otro nodo recibe esa posicion y transmite instrucciones que indican a la interfase delos motores hacia donde se moveran.

El codigo completo esta en el anexo. En el parte siguiente se explica el funcionamiento del codigoen detalle.

3.1. Procesamiento del imagen

El nodo de procesamiento tiene algunas interfases para recibir y transmitir datos.

image t ranspor t : : Subsc r ibe r image sub ;image t ranspor t : : Pub l i sher image pub ;

ro s : : Pub l i she r f a ce pub ;

Los primeros son para recibir las imagenes del la camera y transmitir las imagenes despues delprocesamiento para ver los resultados. La ultima interfase transmite la posicion de la cara.

// Subsc r i b e to input v ideo f eed and pu b l i s h output v ideo f eedimage sub = i t . su b s c r i b e ( ”/camera/ rgb/ image raw” , 1 ,

&ImageProcessor : : imageCb , this ) ;image pub = i t . a d v e r t i s e ( ”/ image proce s so r / output v ideo ” , 1 ) ;

// Pub l i sh face p o s i t i o nf a c e pub = nh . adve r t i s e<std msgs : : Float32MultiArray >(”/ image proce s so r / f a c e p o s i t i o n ” , 1 ) ;

Cada vez que recibe una imagen nueva, se llama a la funcion ImageProcessor::imageCb(). Se guardauna copia de la imagen y llama la funcion ImageProcessor::get faces().

void ImageProcessor : : imageCb ( const sensor msgs : : ImageConstPtr& msg){

cv br idge : : CvImagePtr cv pt r ;try{

cv pt r = cv br idge : : toCvCopy (msg , sensor msgs : : image encodings : : BGR8) ;}catch ( cv br idge : : Exception& e ){

ROS ERROR( ” cv br idge except ion : %s ” , e . what ( ) ) ;return ;

}

5

frame = cv ptr−>image ;std : : vector< cv : : Rect <int> > f a c e s = g e t f a c e s ( ) ;

La funcion ImageProcessor::get faces() usa un filtro cascade de Haar para reconocer caras. Ese filtrobusca grupos de caracterısticas (features), que forman la cara. Una librerıa de features tıpicos formaparte de opencv. Para realizar la tarea, primero se inicializa el filtro con el metodo por defecto de lalibrerıa y luego se usa para identificar todas las caras en la imagen.

// Haar−cascade f o r f i n d i n g f a c e sh a a r c l a s s i f i e r . load ( fn haar ) ;

s td : : vector< cv : : Rect <int> > ImageProcessor : : g e t f a c e s ( ){

std : : vector< cv : : Rect <int> > f a c e s ;

// Convert the current frame to g ray s ca l e :cv : : Mat gray ;cvtColor ( frame , gray , CV BGR2GRAY) ;e q u a l i z e H i s t ( gray , gray ) ;

// Find the f a c e s in the frame :h a a r c l a s s i f i e r . d e t e c tMu l t iS ca l e ( gray , f ace s , 1 . 1 , 6 , 0 |CV HAAR SCALE IMAGE, cv : : S i z e ( 1 5 , 1 5 ) ) ;// At t h i s po in t you have the p o s i t i o n o f the f a c e s in// f a c e s .

return f a c e s ;}

Despues el programa busca la cara mas grande, porque probablemente es la cara mas cercana.

cv : : Rect face max ;

for (unsigned int i = 0 ; i < f a c e s . s i z e ( ) ; i++) {// Process f ace by face :cv : : Rect f a c e i = f a c e s [ i ] ;i f ( i ==0) face max = f a c e i ;i f ( f a c e i . he ight ∗ f a c e i . width > face max . he ight ∗ face max . width )

face max = f a c e i ;

//DEBUGr e c t a n g l e ( frame , f a c e i , CV RGB(0 , 255 ,0 ) , 1 ) ;

}

Para amortiguar errores no se usa la posicion de las caras directamente, sino que usa un filtro deKalman. Se inicializa como se describe en:

// Kalman− f i l t e r f o r d ea l i n g wi th no i se e t c .KF. i n i t ( 2 , 2 , 0 ) ;

KF. t r a n s i t i o n M a t r i x = ∗( cv : : Mat <f loat >(2 , 2) << 1 ,0 , 0 , 1 ) ;s e t I d e n t i t y (KF. measurementMatrix ) ;s e t I d e n t i t y (KF. processNoiseCov , cv : : S ca l a r : : a l l (1 e−5)) ;s e t I d e n t i t y (KF. measurementNoiseCov , cv : : S ca l a r : : a l l (1 e−8)) ;s e t I d e n t i t y (KF. errorCovPost , cv : : S ca l a r : : a l l ( 1 ) ) ;

Tiene dos variables, la posicion de la cara en los ejes Y y Z. Las matrices del processNoiseCov ymeasurementNoiseCov afectan cuan rapido responde el filtro cuando la cara se mueve.

EL filtro predice la proxima posicion y la transmite al controlador del motor.

i f ( num no face < 10 | | f a c e s . s i z e ( ) ){

// i f ( num no face >= 10)

6

// i n i t i a l i z e KF ( ) ;

cv : : Mat p r e d i c t i o n = KF. p r e d i c t ( ) ;

f loat p r e d i c t y = p r e d i c t i o n . at<f loat >(0) ;p r e d i c t y −= frame . s i z e ( ) . width / 2 ;

f loat p r e d i c t z = p r e d i c t i o n . at<f loat >(1) ;

// ROS INFO(” face : x=%f , y=%f , z=%f ” ,0 .0 , p r ed i c t y , p r e d i c t z ) ;std msgs : : Float32Mult iArray array ;//Clear arrayarray . data . c l e a r ( ) ;

array . data . push back ( 0 ) ;array . data . push back ( p r e d i c t y ) ;array . data . push back ( p r e d i c t z ) ;

ROS INFO( ” stream : x= %f , y= %f , z= %f ” , array . data . at ( 0 ) , array . data . at ( 1 ) , array . data . at ( 2 ) ) ;f a c e pub . pub l i sh ( array ) ;

}

Despues, si la imagen actual tiene una cara, se corrige el filtro, y si no, cuenta las imagenes sincara para terminar la transmision de la posicion.

i f ( ! f a c e s . s i z e ( ) ) // i f no face update counter{

num no face ++;}else // e l s e r e s e t counter and update Kalman f i l t e r{

num no face = 0 ;int c en t e r y = face max . x + face max . width / 2 ;int c e n t e r z = face max . y + face max . he ight /2 ;cv : : Mat measurement = ∗( cv : : Mat <f loat >(2 , 1) << center y , c e n t e r z ) ;KF. c o r r e c t ( measurement ) ;

Al final se transmite la imagen procesada.

cv ptr−>image = frame ;// Output modi f ied v ideo streamimage pub . pub l i sh ( cv ptr−>toImageMsg ( ) ) ;

3.2. Control de los motores

Para comunicarse con los motores, el sistema usa un mensaje especial geometry msgs::TwistPtr cmd.Se tienen 6 velocidades, 3 lineales y 3 angulares. El Robot Turtlebot solamente puede usar 2 de ellas,las velocidades lineales en direccion X y angular respecto de Z. Cada 0,1 seg transmite las velocidadesactuales a los motores

void MotorContro l ler : : sp in ( ){ro s : : Rate l o o p r a t e ( 1 0 ) ;while ( ! q u i t r e q u e s t e d && ros : : ok ( ) ){

// Avoid spamming robo t wi th cont inuous zero−v e l o c i t y messagesi f ( ( cmd−>l i n e a r . x != 0 . 0 ) | | (cmd−>l i n e a r . y != 0 . 0 ) | | (cmd−>l i n e a r . z != 0 . 0 ) | |(cmd−>angular . x != 0 . 0 ) | | (cmd−>angular . y != 0 . 0 ) | | (cmd−>angular . z != 0 . 0 ) ){

v e l o c i t y p u b l i s h e r . pub l i sh (cmd ) ;l a s t z e r o v e l s e n t = fa l se ;

7

}else i f ( l a s t z e r o v e l s e n t == fa l se ){

v e l o c i t y p u b l i s h e r . pub l i sh (cmd ) ;l a s t z e r o v e l s e n t = true ;

}

accept incoming = true ;r o s : : spinOnce ( ) ;l o o p r a t e . s l e e p ( ) ;

}

El Control usado es un control proporcional. Se recibe la posicion de la cara mas cercana y actualizalas velocidades. El desplazamiento en el eje Y es el error de control. El valor proporcional K delcontrolador se calcula con respecto a la velocidad maxima angular vel y un desplazamiento tıpico enpixels que se toma de 200px.

void MotorContro l ler : : r e c i e v e d P o s i t i o n ( const std msgs : : Float32Mult iArray msg){

f loat pos x = msg . data . at ( 0 ) ;f loat pos y = msg . data . at ( 1 ) ;f loat pos z = msg . data . at ( 2 ) ;

ROS INFO( ” f a c e in x: %f , y : %f , z : %f ” , pos x , pos y , pos z ) ;

i f ( pos y > −10 && pos y < 10)se tAngu la rVe loc i ty ( 0 . 0 ) ;

elses e tAngu la rVe loc i ty (− a n g u l a r v e l /200 ∗ pos y ) ;

i f ( pos x > 50)s e t L i n e a r V e l o c i t y ( l i n e a r v e l ) ;

else i f ( pos x <10)s e t L i n e a r V e l o c i t y ( 0 . 0 ) ;

}

3.3. Resultado

La realizacion de la tarea es muy simple, especialmente porque opencv tiene muchas funcionesimplementadas, por ejemplo los filtros de Haar o Kalman. Un problema del sistema operativo es que esmuy grande y por eso alguna veces opera lentamente. Todas las partes del programa: el procesamiento yel control, funcionan muy bien, pero cuando transmite las imagenes en formato crudo, solamente puedeactualizar los instrucciones del robot cada unos segundos. Serıa mucho mejor, si utilizara imagenescomprimidas.

8

Anexo

ImageProcessor

/∗∗ ImageProcessor . hpp∗∗ Created on : Oct 23 , 2014∗ Author : chemluth∗/

#ifndef IMAGE PROCESSOR SRC IMAGEPROCESSOR HPP#define IMAGE PROCESSOR SRC IMAGEPROCESSOR HPP

#include <ro s / ro s . h>#include <image t ranspor t / image t ranspor t . h>#include <cv br idge / cv br idge . h>#include <sensor msgs / image encodings . h>

#include ”opencv2/ core / core . hpp”#include ”opencv2/ cont r i b / cont r i b . hpp”#include ”opencv2/ h ighgu i / h ighgu i . hpp”#include ”opencv2/ imgproc/ imgproc . hpp”#include ”opencv2/ ob jde t e c t / ob jde t e c t . hpp”#include ”opencv2/ video / t rack ing . hpp”

class ImageProcessor{private :

r o s : : NodeHandle nh ;image t ranspor t : : ImageTransport i t ;image t ranspor t : : Subsc r ibe r image sub ;image t ranspor t : : Pub l i sher image pub ;

ro s : : Pub l i she r f a ce pub ;

cv : : C a s c a d e C l a s s i f i e r h a a r c l a s s i f i e r ;cv : : KalmanFilter KF;

int num no face ;

cv : : Mat frame ;

std : : vector< cv : : Rect <int> > g e t f a c e s ( ) ;

public :ImageProcessor ( std : : s t r i n g fn haar = ”/ usr / share /opencv/ haarcascades / h a a r c a s c a d e f r o n t a l f a c e a l t 2 . xml” ) ;˜ ImageProcessor ( ) ;

void imageCb ( const sensor msgs : : ImageConstPtr& msg ) ;

9

} ;

#endif /∗ IMAGE PROCESSOR SRC IMAGEPROCESSOR HPP ∗/

10

/∗∗ ImageProcessor . cpp∗∗ Created on : Oct 23 , 2014∗ Author : chemluth∗/

#include <std msgs / MultiArrayLayout . h>#include <std msgs / MultiArrayDimension . h>#include <std msgs / Float32Mult iArray . h>

#include ” . . / i n c lude / v i s i o n p r o j e c t / ImageProcessor . hpp”

ImageProcessor : : ImageProcessor ( std : : s t r i n g fn haar ) : i t ( nh ){

// Subsc r i b e to input v ideo f eed and pu b l i s h output v ideo f eedimage sub = i t . su b s c r i b e ( ”/camera/ rgb/ image raw” , 1 ,

&ImageProcessor : : imageCb , this ) ;image pub = i t . a d v e r t i s e ( ”/ image proce s so r / output v ideo ” , 1 ) ;

// Pub l i sh face p o s i t i o nf a c e pub = nh . adve r t i s e<std msgs : : Float32MultiArray >(”/ image proce s so r / f a c e p o s i t i o n ” , 1 ) ;

// Haar−cascade f o r f i n d i n g f a c e sh a a r c l a s s i f i e r . load ( fn haar ) ;

// Kalman− f i l t e r f o r d ea l i n g wi th no i se e t c .KF. i n i t ( 2 , 2 , 0 ) ;

KF. t r a n s i t i o n M a t r i x = ∗( cv : : Mat <f loat >(2 , 2) << 1 ,0 , 0 , 1 ) ;s e t I d e n t i t y (KF. measurementMatrix ) ;s e t I d e n t i t y (KF. processNoiseCov , cv : : S ca l a r : : a l l (1 e−5)) ;s e t I d e n t i t y (KF. measurementNoiseCov , cv : : S ca l a r : : a l l (1 e−8)) ;s e t I d e n t i t y (KF. errorCovPost , cv : : S ca l a r : : a l l ( 1 ) ) ;

num no face = 0 ;

}

ImageProcessor : : ˜ ImageProcessor ( ){}

std : : vector< cv : : Rect <int> > ImageProcessor : : g e t f a c e s ( ){

std : : vector< cv : : Rect <int> > f a c e s ;

// Convert the current frame to g ray s ca l e :cv : : Mat gray ;cvtColor ( frame , gray , CV BGR2GRAY) ;e q u a l i z e H i s t ( gray , gray ) ;

// Find the f a c e s in the frame :h a a r c l a s s i f i e r . d e t e c tMu l t iS ca l e ( gray , f ace s , 1 . 1 , 6 , 0 |CV HAAR SCALE IMAGE, cv : : S i z e ( 1 5 , 1 5 ) ) ;// At t h i s po in t you have the p o s i t i o n o f the f a c e s in// f a c e s .

return f a c e s ;}

11

void ImageProcessor : : imageCb ( const sensor msgs : : ImageConstPtr& msg){

cv br idge : : CvImagePtr cv pt r ;try{

cv pt r = cv br idge : : toCvCopy (msg , sensor msgs : : image encodings : : BGR8) ;}catch ( cv br idge : : Exception& e ){

ROS ERROR( ” cv br idge except ion : %s ” , e . what ( ) ) ;return ;

}

frame = cv ptr−>image ;std : : vector< cv : : Rect <int> > f a c e s = g e t f a c e s ( ) ;

i f ( num no face < 10 | | f a c e s . s i z e ( ) ){

// i f ( num no face >= 10)// i n i t i a l i z e KF ( ) ;

cv : : Mat p r e d i c t i o n = KF. p r e d i c t ( ) ;

f loat p r e d i c t y = p r e d i c t i o n . at<f loat >(0) ;p r e d i c t y −= frame . s i z e ( ) . width / 2 ;

f loat p r e d i c t z = p r e d i c t i o n . at<f loat >(1) ;

// ROS INFO(” face : x=%f , y=%f , z=%f ” ,0 .0 , p r ed i c t y , p r e d i c t z ) ;std msgs : : Float32Mult iArray array ;//Clear arrayarray . data . c l e a r ( ) ;

array . data . push back ( 0 ) ;array . data . push back ( p r e d i c t y ) ;array . data . push back ( p r e d i c t z ) ;

ROS INFO( ” stream : x= %f , y= %f , z= %f ” , array . data . at ( 0 ) , array . data . at ( 1 ) , array . data . at ( 2 ) ) ;f a c e pub . pub l i sh ( array ) ;

}

// f i nd b i g g e s t f acecv : : Rect face max ;

for (unsigned int i = 0 ; i < f a c e s . s i z e ( ) ; i++) {// Process f ace by face :cv : : Rect f a c e i = f a c e s [ i ] ;i f ( i ==0) face max = f a c e i ;i f ( f a c e i . he ight ∗ f a c e i . width > face max . he ight ∗ face max . width )

face max = f a c e i ;

//DEBUGr e c t a n g l e ( frame , f a c e i , CV RGB(0 , 255 ,0 ) , 1 ) ;

}

i f ( ! f a c e s . s i z e ( ) ) // i f no face update counter{

12

num no face ++;}else // e l s e r e s e t counter and update Kalman f i l t e r{

num no face = 0 ;int c en t e r y = face max . x + face max . width / 2 ;int c e n t e r z = face max . y + face max . he ight /2 ;cv : : Mat measurement = ∗( cv : : Mat <f loat >(2 , 1) << center y , c e n t e r z ) ;KF. c o r r e c t ( measurement ) ;

}

cv ptr−>image = frame ;// Output modi f ied v ideo streamimage pub . pub l i sh ( cv ptr−>toImageMsg ( ) ) ;

}

int main ( int argc , char∗∗ argv ){

ro s : : i n i t ( argc , argv , ” image proce s so r ” ) ;ImageProcessor ip ;ro s : : sp in ( ) ;return 0 ;

}

13

MotorController

/∗∗ MotorContro l l er . hpp∗∗ Created on : 05.11.2014∗ Author : chemluth∗/

#ifndef VISION PROJECT INCLUDE VISION PROJECT MOTORCONTROLLER HPP#define VISION PROJECT INCLUDE VISION PROJECT MOTORCONTROLLER HPP

#include <ro s / ro s . h>#include <termios . h> // f o r keyboard input#include <e c l / threads . hpp>#include <geometry msgs /Twist . h> // f o r v e l o c i t y commands#include <geometry msgs /TwistStamped . h> // f o r v e l o c i t y commands#include <std msgs / MultiArrayLayout . h>#include <std msgs / MultiArrayDimension . h>#include <std msgs / Float32Mult iArray . h>

class MotorContro l l er{public :

/∗ Constructor and Des truc tor ∗/MotorContro l ler ( ) ;˜ MotorContro l l er ( ) ;bool i n i t ( ) ;

/∗ Runtime ∗/void sp in ( ) ;

private :

r o s : : Subsc r ibe r p o s i t i o n s u b s c r i b e r ;ro s : : Pub l i she r v e l o c i t y p u b l i s h e r ;ro s : : Pub l i she r motor power pub l i sher ;

bool l a s t z e r o v e l s e n t ;bool accept incoming ;bool power s tatus ;bool w a i t f o r c o n n e c t i o n ;

geometry msgs : : TwistPtr cmd ;geometry msgs : : TwistStampedPtr cmd stamped ;double l i n e a r v e l ;double a n g u l a r v e l ;s td : : s t r i n g name ;

/∗ Commands ∗/void enable ( ) ;void d i s a b l e ( ) ;/∗vo id incrementLinearVe loc i ty ( ) ;vo id decrementLinearVe loc i ty ( ) ;vo id incrementAngu larVe loc i ty ( ) ;vo id decrementAngularVeloc i ty ( ) ;∗/void r e s e t V e l o c i t y ( ) ;void s e t L i n e a r V e l o c i t y ( f loat l i n e a r x = 0 . 0 ) ;

14

void s e tAngu la rVe loc i ty ( f loat l i n e a r x = 0 . 0 ) ;

/∗ Subsc r i b e r Ca l l back ∗/void r e c i e v e d P o s i t i o n ( const std msgs : : Float32Mult iArray msg ) ;

/∗ Keyboard Inputs ∗/void keyboardInputLoop ( ) ;

bool q u i t r e q u e s t e d ;int k e y f i l e d e s c r i p t o r ;struct termios o r i g i n a l t e r m i n a l s t a t e ;e c l : : Thread thread ;

} ;

#endif /∗ VISION PROJECT INCLUDE VISION PROJECT MOTORCONTROLLER HPP ∗/

15

/∗∗ MotorContro l l er . cpp∗∗ Created on : 05.11.2014∗ Author : chemluth∗/

#include <ro s / ro s . h>#include <e c l / time . hpp>#include <e c l / except i ons . hpp>#include <s t d s r v s /Empty . h>#include <kobuki msgs /MotorPower . h>#include ” std msgs / MultiArrayLayout . h”#include ” std msgs / MultiArrayDimension . h”#include ” std msgs / Float32Mult iArray . h”

#include ” . . / i n c lude / v i s i o n p r o j e c t / MotorContro l l er . hpp”

MotorContro l l er : : MotorContro l ler ( ) : l a s t z e r o v e l s e n t ( true ) , // avoid zero−v e l messages from the beg inn ingaccept incoming ( true ) ,power s tatus ( fa l se ) ,w a i t f o r c o n n e c t i o n ( true ) ,cmd(new geometry msgs : : Twist ( ) ) ,cmd stamped (new geometry msgs : : TwistStamped ( ) ) ,l i n e a r v e l ( 0 . 5 ) ,a n g u l a r v e l ( 0 . 1 ) ,q u i t r e q u e s t e d ( fa l se ) ,k e y f i l e d e s c r i p t o r (0 )

{t c g e t a t t r ( k e y f i l e d e s c r i p t o r , &o r i g i n a l t e r m i n a l s t a t e ) ; // ge t t e rmina l p r o p e r t i e s

}

MotorContro l l er : : ˜ MotorContro l ler ( ){

t c s e t a t t r ( k e y f i l e d e s c r i p t o r , TCSANOW, &o r i g i n a l t e r m i n a l s t a t e ) ;}

/∗∗∗ @br ie f I n i t i a l i s e s the node .∗/bool MotorContro l ler : : i n i t ( ){

ro s : : NodeHandle nh( ”˜” ) ;name = nh . getUnresolvedNamespace ( ) ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Parameters∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/nh . getParam ( ” l i n e a r v e l ” , l i n e a r v e l ) ;nh . getParam ( ” a n g u l a r v e l ” , a n g u l a r v e l ) ;nh . getParam ( ” w a i t f o r c o n n e c t i o n ” , w a i t f o r c o n n e c t i o n ) ;

ROS INFO STREAM( ” MotorContro l l er : us ing l i n e a r ve l [ ” << l i n e a r v e l << ” ] . ” ) ;ROS INFO STREAM( ” MotorContro l l er : us ing angular ve l [ ” << a n g u l a r v e l << ” ] . ” ) ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Sub s c r i b e r s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/p o s i t i o n s u b s c r i b e r = nh . su b s c r i b e ( ”/ image proce s so r / f a c e p o s i t i o n ” , 1 , &MotorContro l l er : : r e c i ev ed Po s i t i o n , this ) ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗

16

∗∗ Pub l i s h e r s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/v e l o c i t y p u b l i s h e r = nh . adve r t i s e<geometry msgs : : Twist>(” cmd vel ” , 1 ) ;motor power pub l i sher = nh . adve r t i s e<kobuki msgs : : MotorPower>(”motor power” , 1 ) ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Ve l o c i t i e s∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/cmd−>l i n e a r . x = 0 . 0 ;cmd−>l i n e a r . y = 0 . 0 ;cmd−>l i n e a r . z = 0 . 0 ;cmd−>angular . x = 0 . 0 ;cmd−>angular . y = 0 . 0 ;cmd−>angular . z = 0 . 0 ;

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Wait f o r connect ion∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/i f ( ! w a i t f o r c o n n e c t i o n )

return true ;

e c l : : M i l l i S l e e p m i l l i s l e e p ;int count = 0 ;bool connected = fa l se ;while ( ! connected ){

i f ( motor power pub l i sher . getNumSubscribers ( ) > 0){connected = true ;break ;

}i f ( count == 6){

connected = fa l se ;break ;

}else {

ROS WARN STREAM( ” C o n t r o l l e r : could not connect , t r y ing again a f t e r 500ms . . . ” ) ;try{

m i l l i s l e e p ( 5 0 0 ) ;}catch ( e c l : : StandardException e ){

ROS ERROR STREAM( ”Waiting has been in t e r rup t ed . ” ) ;ROS DEBUG STREAM( e . what ( ) ) ;return fa l se ;

}++count ;

}}

i f ( ! connected ){ROS ERROR( ” C o n t r o l l e r : could not connect . ” ) ;ROS ERROR( ” C o n t r o l l e r : check remappings f o r enable / d i s a b l e t o p i c s ) . ” ) ;

}else {

kobuki msgs : : MotorPower power cmd ;power cmd . s t a t e = kobuki msgs : : MotorPower : :ON;motor power pub l i sher . pub l i sh ( power cmd ) ;ROS INFO( ” C o n t r o l l e r : connected . ” ) ;power s tatus = true ;

}

17

// s t a r t keyboard input threadthread . s t a r t (&MotorContro l l er : : keyboardInputLoop , ∗ this ) ;return true ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Implementation [ Spin ]∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗//∗∗∗ @br ie f Worker thread loop ; sends curren t v e l o c i t y command at a f i x e d ra t e .∗∗ I t a l s o proces s ros f unc t i on s as w e l l as abo r t i ng when reque s t ed .∗/void MotorContro l ler : : sp in ( ){

ro s : : Rate l o o p r a t e ( 1 0 ) ;while ( ! q u i t r e q u e s t e d && ros : : ok ( ) ){

// Avoid spamming robo t wi th cont inuous zero−v e l o c i t y messagesi f ( ( cmd−>l i n e a r . x != 0 . 0 ) | | (cmd−>l i n e a r . y != 0 . 0 ) | | (cmd−>l i n e a r . z != 0 . 0 ) | |(cmd−>angular . x != 0 . 0 ) | | (cmd−>angular . y != 0 . 0 ) | | (cmd−>angular . z != 0 . 0 ) ){

v e l o c i t y p u b l i s h e r . pub l i sh (cmd ) ;l a s t z e r o v e l s e n t = fa l se ;

}else i f ( l a s t z e r o v e l s e n t == fa l se ){

v e l o c i t y p u b l i s h e r . pub l i sh (cmd ) ;l a s t z e r o v e l s e n t = true ;

}

accept incoming = true ;r o s : : spinOnce ( ) ;l o o p r a t e . s l e e p ( ) ;

}i f ( q u i t r e q u e s t e d ){ // ros node i s s t i l l ok , send a d i s a b l e command

d i s a b l e ( ) ;}else{

// j u s t in case we got here not v ia a keyboard q u i t r e que s tq u i t r e q u e s t e d = true ;thread . cance l ( ) ;

}thread . j o i n ( ) ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Implementation [ KeyboardInput ]∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

void MotorContro l ler : : keyboardInputLoop ( ){

struct termios raw ;memcpy(&raw , &o r i g i n a l t e r m i n a l s t a t e , s izeof ( struct termios ) ) ;raw . c l f l a g &= ˜(ICANON | ECHO) ;// Se t t i n g a new l ine , then end o f f i l eraw . c c c [VEOL] = 1 ;raw . c c c [VEOF] = 2 ;t c s e t a t t r ( k e y f i l e d e s c r i p t o r , TCSANOW, &raw ) ;puts ( ”Reading from keyboard ” ) ;puts ( ”−−−−−−−−−−−−−−−−−−−−−−−−−−−” ) ;

18

puts ( ” r : r e s e t l i n e a r / angular v e l o c i t i e s . ” ) ;puts ( ”d : d i s a b l e motors . ” ) ;puts ( ”e : enable motors . ” ) ;puts ( ”q : qu i t . ” ) ;char c ;while ( ! q u i t r e q u e s t e d ){

i f ( read ( k e y f i l e d e s c r i p t o r , &c , 1) < 0){

per ro r ( ” read char f a i l e d ( ) : ” ) ;e x i t (−1);

}

switch ( c ){

case ’ r ’ :{

r e s e t V e l o c i t y ( ) ;break ;

}case ’ q ’ :{

q u i t r e q u e s t e d = true ;break ;

}case ’ d ’ :{

d i s a b l e ( ) ;break ;

}case ’ e ’ :{

enable ( ) ;break ;

}default :{

break ;}

}}

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Implementation [ Sub s c r i b e r Ca l l back ]∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

void MotorContro l l er : : r e c i e v e d P o s i t i o n ( const std msgs : : Float32Mult iArray msg){

f loat pos x = msg . data . at ( 0 ) ;f loat pos y = msg . data . at ( 1 ) ;f loat pos z = msg . data . at ( 2 ) ;

ROS INFO( ” f a c e in x: %f , y : %f , z : %f ” , pos x , pos y , pos z ) ;

i f ( pos y > −10 && pos y < 10)se tAngu la rVe loc i ty ( 0 . 0 ) ;

elses e tAngu la rVe loc i ty (− a n g u l a r v e l /200 ∗ pos y ) ;

19

i f ( pos x > 50)s e t L i n e a r V e l o c i t y ( l i n e a r v e l ) ;

else i f ( pos x <10)s e t L i n e a r V e l o c i t y ( 0 . 0 ) ;

}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Implementation [Commands ]∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

/∗∗∗ @br ie f D i sab l e s commands to the nav i ga t i on system .∗∗ This does the f o l l ow i n g t h i n g s :∗∗ − Disab l e s power to the nav i ga t i on motors ( v ia device manager ) .∗ @param msg∗/void MotorContro l ler : : d i s a b l e ( ){

cmd−>l i n e a r . x = 0 . 0 ;cmd−>angular . z = 0 . 0 ;v e l o c i t y p u b l i s h e r . pub l i sh (cmd ) ;accept incoming = fa l se ;i f ( power s tatus ){

ROS INFO( ” C o n t r o l l e r : d i s a b l i n g power to the dev i c e ’ s motor system . ” ) ;kobuki msgs : : MotorPower power cmd ;power cmd . s t a t e = kobuki msgs : : MotorPower : : OFF;motor power pub l i sher . pub l i sh ( power cmd ) ;power s tatus = fa l se ;

}else{

ROS WARN( ” C o n t r o l l e r : Motor system has a l r eady been powered down . ” ) ;}

}

/∗∗∗ @br ie f Reset /re−enab l e the nav i ga t i on system .∗∗ − r e s e t s the command v e l o c i t i e s .∗ − r eenab l e power i f not enab led .∗/void MotorContro l l er : : enable ( ){

accept incoming = fa l se ;cmd−>l i n e a r . x = 0 . 0 ;cmd−>angular . z = 0 . 0 ;v e l o c i t y p u b l i s h e r . pub l i sh (cmd ) ;i f ( ! power s tatus ){

ROS INFO( ” C o n t r o l l e r : Enabling power to the dev i c e subsystem . ” ) ;kobuki msgs : : MotorPower power cmd ;power cmd . s t a t e = kobuki msgs : : MotorPower : :ON;motor power pub l i sher . pub l i sh ( power cmd ) ;power s tatus = true ;

}else {

ROS WARN( ” C o n t r o l l e r : Device has a l r eady been powered up . ” ) ;

20

}}

void MotorContro l ler : : r e s e t V e l o c i t y ( ){// i f ( power s ta tu s ){

cmd−>angular . z = 0 . 0 ;cmd−>l i n e a r . x = 0 . 0 ;ROS INFO STREAM( ” C o n t r o l l e r : r e s e t l i n e a r / angular v e l o c i t i e s . ” ) ;

// }// e l s e {// ROSWARN STREAM(” Con t ro l l e r : motors are not ye t powered up . ” ) ;// }}

void MotorContro l ler : : s e t L i n e a r V e l o c i t y ( f loat l i n e a r x ){// i f ( power s ta tu s ){

cmd−>l i n e a r . x = l i n e a r x ;ROS INFO STREAM( ” C o n t r o l l e r : Set l i n e a r / angular v e l o c i t i e s . ” ) ;

// }// e l s e {// ROSWARN STREAM(” Con t ro l l e r : motors are not ye t powered up . ” ) ;// }}

void MotorContro l ler : : s e tAngu la rVe loc i ty ( f loat angu la r z ){// i f ( power s ta tu s ){

cmd−>angular . z = angu la r z ;ROS INFO STREAM( ” C o n t r o l l e r : Set l i n e a r / angular v e l o c i t i e s . ” ) ;

// }// e l s e {// ROSWARN STREAM(” Con t ro l l e r : motors are not ye t powered up . ” ) ;// }}

/∗ ∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ Main Programm∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗ ∗/

int main ( int argc , char ∗∗ argv ){

ro s : : i n i t ( argc , argv , ” t u r t l e b o t f o l l o w e r ” ) ;MotorContro l ler f o l l o w e r ;i f ( f o l l o w e r . i n i t ( ) ){

f o l l o w e r . sp in ( ) ;}else{

ROS ERROR STREAM( ”Couldn ’ t i n i t i a l i s e Turt l ebotFo l lower ! ” ) ;}ROS INFO STREAM( ”Program e x i t i n g ” ) ;return 0 ;

}

21