Interesting Survey about different Robotic Software Frameworks for Distributed Robotics

This work identifies many aspects of the distributed robotics and compare ROS and other Robotic Frameworks for distributed robotics. Here I show the abstract:

Abstract

Robotics is an area of research in which the paradigm of Multi-Agent Systems (MAS) can prove to be highly useful. Multi-Agent Systems come in the form of cooperative robots in a team, sensor networks based on mobile robots, and robots in Intelligent Environments, to name but a few. However, the development of Multi-Agent Robotic Systems (MARS) still presents major challenges. Over the past decade, a high number of Robotics Software Frameworks (RSFs) have appeared which propose some solutions to the most recurrent problems in robotics. Some of these frameworks, such as ROS, YARP, OROCOS, ORCA, Open-RTM, and Open-RDK, possess certain characteristics and provide the basic infrastructure necessary for the development of MARS. The contribution of this work is the identification of such characteristics as well as the analysis of these frameworks in comparison with the general-purpose Multi-Agent System Frameworks (MASFs), such as JADE and Mobile-C.

Keywords

  • Robotics;
  • MAS;
  • Agents;
  • Software frameworks;
  • Middleware;
  • Architecture

Cambiando el marco de referencia de una distribución Gaussiana

El trabajo de funciones probabilísticas gausianas en el espacio es algo muy habitual en robótica. Este tipo de distribuciones permite expresar una posición o pose (por ejemplo de un objeto u objetivo) de la cual se tiene cierta incertidumbre. Desde mi punto de vista se trata de una temática bastante interesante y me está permitiendo profundizar en algunos conceptos matemáticos como autovalores, números complejos y quaterniones. Otros posts previos que realicé en el pasado con temáticas relacionadas son:

Una función gausiana está compuesta de su vector media \mu y su matriz de covarianza \Sigma y se denota como P(x)\sim\mathcal{N}(\mu,\Sigma). El problema consiste en modificar estos parámetros para pasar del marco de referencia F_{A} al marco de referencia F_{B}. Estos marcos de referencia están descritos por una transformación lineal que incluye rotación y translación respecto a un marco de referencia universal, de modo que:

F_{A}\equiv(R_{A},T_{A})

F_{B}\equiv(R_{B},T_{B})

Cambio del marco de referencia del vector media \mu

El cambio de referencia del vector media es trivial dada una transformación TF entre marcos de referencia básicamente consiste en aplicar la transformación lineal al vector \mu_{A}:

\mu_{B} = R \mu_{A} + T

Donde:

R=R_{B}R_{A}^{-1}
T=T_{B}-R_{B}T_{A}

De echo en ocasiones los valores universales de los marcos de referencia R_{A}, T_{A}, R_{B}  y T_{B} no son conocidos, sino que directamente la relación entre ambos marcos TF\equiv (R,T).

Cambio del marco de referencia de la matriz de Covarianza \Sigma

¿Pero que pasa con la matriz de covarianza? En el caso de un problema unidimensional, el cambio de referencia del origen de coordenadas no afectaría a la matriz de covarianza \Sigma=\begin{bmatrix} \sigma^{2} \end{bmatrix} (que es la varianza). Por ejemplo: P(x)\sim\mathcal{N}(\mu_{A}=3,\Sigma_{A}=5) se convertiría en P(x)\sim\mathcal{N}(\mu_{B}=-7,\Sigma_{B}=5) si cambiaramos el origen de coordenadas desde el O=0 a O=10.

Para una función gausiana 2D o 3D es mucho más complicado. La matriz de covarianza representa la orientación de la distribución además de la  escala de la variabilidad en dentro de los ejes principales de esta rotación. A la derecha se muestra una figura de una función gausiana 2D. En una función gaussiana (creo, a ver si un matemático me lo confirma) que la probabilidad se extiende en N vectores ortogonales (autovectores) con diferentes escalas (autovalores) verificando que:

\Sigma = E (l I) E^{t}

Siendo l el vector de autovalores, el punto curioso es que la matriz E es una matriz de rotación cuyas columnas son los autovectores de \Sigma.  Es intuitivo ver que si el marco de referencia cambia, las coordenadas de estos autovectores cambiará, sin embargo la escala (autovalores) se mantendrá constante. Parece entonces sencillo.

Si estamos trabajando en el marco de referencia F_{A}F_{B} es conocido, entonces basta con cambiar el marco de referencia de los autovectores:

E_{B}=RE_{A}

\Sigma_{B}=RE_{A}(lI)(RE_{A})^{t}

o lo que es lo mismo si tenemos la información completa de F_{A}F_{B}:

\Sigma_{B}=R_{B}R_{A}^{-1}E_{A}(lI)(R_{B}R_{A}^{-1}E_{A})^{t}

Casos particulares que aún no se resolver

Sin embargo existen casos particulares y estos son los que a mi, personalmente me inquietan y aún no tengo claro como resolver:

  1. ¿Qué ocurre si la matriz de covarianza no es diagonalizable? Por ejemplo, se tiene total certeza de su posición en X, su posición en Y y algo de incertidumbre en Z. Siendo \Sigma_{A}=\begin{bmatrix}  0 & 0 & 0\\  0 & 0 & 0\\  0 & 0 & 2.3  \end{bmatrix} ¿Podría obtenerse la matriz E_{A} si solo tenemos un autovector?
  2. ¿Qué ocurre cuando trabajamos (en lugar de con un vector “posición” en 3D) con una “pose en el espacio” xyz+rpy 6D con una matriz de covarianza de 36 valores, donde se mezclan distintas unidades? Este tipo de representación gausiana es también muy utilizada en robótica, sin embargo, el abordaje del problema de cambio de sistema referencia de este tipo de distribuciones es algo que no he logrado localizar en ningún sitio. Parece un caso muy extraño dado que además los valores de rpy dependen en sí mismos del los ejes x,y,z del marco de referencia actual.
Ideas problema 1:
En el ejemplo tratado parece claro que el único autovector existente es v_0=\begin{vmatrix}\\ 0\\ 0\\ 1 \end{vmatrix} y su autovector \lambda_0=2.3 ya que verifica que \Sigma v_0 = \lambda v_0 . Intuyo que dada la precondición de ortogonalidad de los autovectores de la matriz E(¿inventado?) parece razonable que solo necesitamos un autovector para calcular E. De hecho  cualquier E que verifique que l=\begin{vmatrix}\\ \lambda_0\\ \lambda_1=0\\\lambda_2=0 \end{vmatrix} y sea ortogonal parece suficiente ¿sera correcto este razonamiento?Duda: Pienso que una matriz de covarianza no nula debería tener al menos un autovector real, pero de esto, no estoy seguro. Si fuera así, el problema estaría resuelto.
Ideas problema 2:
Creo que dado que el cambio de coordenadas en robótica solo afectaría a los ejes x, y y z el problema se simplificaría. El vector \mu para x, y y z cambiaría de la misma forma. Por el contrario los valores rpy en lugar de ser rotados serían trasladados. \mu_{B}= R* \mu_{A} + T donde:
R=\begin{bmatrix}  R_{xyz} & \O \\  \O & I\\\end{bmatrix}
y
T=\begin{bmatrix} t_x \\ t_y \\ t_z \\ getRoll(R) \\ getPitch(R) \\ getYaw(R) \\\end{bmatrix}
De esta forma las componentes de \Sigma_A definidas por covarianzas angulares, como \sigma_{row,pitch}=cov(row,pitch) permanecerían constantes tras la transformación TF, mientras que las componentes de la matríz como \sigma_{x,yaw}=cov(x,yaw) cambiarían su valor. Sin embargo, todo esto son por ahora solo intuiciones. ¿es correcto este razonamiento?¿Alguna ayuda?

Basic Rotations with Quaternions

The quaternions is a very interesting mathematical tools that make possible perform rotation operations without using rotation matrices. This method is efficent and secure (avoiding the gimbal lock effect) in dynamic situations. A very interesting explanation about the mathematical foundations and capacities of quaternions can be found here.

The ROS library TF provide several functions to handle quaternions in C++ and Python. There exists many opensource libraries which provides quaternion funciontalities like Eigen, Bullet to name a couple of them.

Rotating a point
ROS works with the right hand axis convention. This axis convention is specially adequate for mobile robots. Here the robot is always looking to the axis-x infinity.

import roslib
roslib.load_manifest('tf')
import tf
from tf.transformations import *

# we want to rotate the point using the x-axis ("roll")
rot=quaternion_from_euler(-numpy.pi/4,0,0)

# the object is located in the y=1. We use the format [x,y,z,w] and w is allways 0 for vectors
vec= [0,1,0,0]

#now we apply the mathematical operation res=q*v*q'. We use this function for multiplication but internally it is just a complex multiplication operation.
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [33]: result
Out[33]: array([ 0. , 0.70710678, -0.70710678, 0. ])

Concatenate Rotations
Here I show how to concatenate multiple rotations using quaternions:

q1=quaternion_from_euler(-numpy.pi/4,0,0)
q2=quaternion_from_euler(numpy.pi/4,0,0)
q3=quaternion_from_euler(-numpy.pi/4,0,0)
q4=quaternion_from_euler(numpy.pi/4,0,numpy.pi/4)

#the full transform is q4*q3*q2*q1
rot= quaternion_multiply(q4,quaternion_multiply(q3,quaternion_multiply(q2,q1)))
result=quaternion_multiply(quaternion_multiply(rot, vec),quaternion_conjugate(rot))

In [86]: result1
Out[86]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [90]: result2
Out[90]: array([ 0., 1., 0., 0.])

In [93]: result3
Out[93]: array([ 0. , 0.70710678, -0.70710678, 0. ])

In [96]: result
Out[96]: array([-0.70710678, 0.70710678, 0. , 0. ])

My robotic-books wish list

Ahí va una lista de libros interesantes para robótica que últimamente he leido/ quiero leer. Aquí la dejo para no olvidarla y por si a alguien le pudiera servir:
Robótica teórica y práctica:
Probabilistic Robotics (Intelligent Robotics & Autonomous Agents Series) de Sebastian Thrun, Wolfram Burgard y Dieter Fox (Tapa dura – 20 septiembre 2005)
Springer Handbook of Robotics by Bruno Siciliano and Oussama Khatib (Jun 27, 2008
Principles of Robot Motion: Theory, Algorithms, and Implementations (Intelligent Robotics and Autonomous Agents series)by Howie Choset, Kevin M. Lynch, Seth Hutchinson and George A. Kantor (May 20, 2005
Estos también he escuchado que son buenos de robótica más teórica:
Robotics: Modelling, Planning and Control (Advanced Textbooks in Control and Signal Processing) by Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani and Giuseppe Oriolo (Feb 11, 2011)
Springer Handbook of Automation by Shimon Y. Nof (Aug 27, 2009)
También otros muy interesantes, más prácticos:
Probabilistic Reasoning and Decision Making in Sensory-Motor Systems (Springer Tracts in Advanced Robotics) de Pierre Bessiere, Christian Laugier y Roland Y. Siegwart(Tapa blanda – 22 octubre 2010)
Arduino:
Arduino Robotics by John-David Warren, Josh Adams and Harald Molle (Jul 14, 2011)
Otros que a mi me interesan relacionados con robótica:
Computational Geometry: Algorithms and Applications by Mark de Berg, Otfried Cheong, Marc van Kreveld and Mark Overmars (Nov 19, 2010)

How to represent a 3D Gaussian Function with ROS rviz

The probabilistic functions are very usual in robotics to represent the certainty of states and observations. The most typical and used probabilistic function is the Gaussian function. Everybody who knows what is this immediately imagine a pretty bell which describes the samples nature.

In the one-dimensional case, a simple tuple (\mu, \sigma^{2}) mean, variance is enough information to imagine how the samples are behaving. This tuple can be also reinterpreted as an interval [\mu - \sigma^{2}, \mu + \sigma^{2}] where the samples most probably are located. For higher dimensions the \mu it is clear, but the equivalent element to describe the diversity, the covariance matrix \Sigma it is much lesser intuitive for most of people. The trick is imagine the ND gaussian bell as NDimensional ellipse. The shape and rotation of this ellipse is described by the covariance matrix. In this way the diversity of the distribution is intuitive.

It is extremely interesting to see and understand the relationship with the rotation and scale of the ellipsoid. If we define R as the rotation transform of the ellipsoid and l is a vector with the variances in the local frame of the ellipse (much easier to imagine) l = \begin{pmatrix}\sigma_{x_1,x_1}^{2} & ... & \sigma_{x_n,x_n}^{2}\end{pmatrix} , then the relationship between R, l and \Sigma is this:

\Sigma = R (l I) R^{t}

This have important implications because if you want to synthesizer a gaussian belief from a theoretical model can be hard imagine the covariance matrix values but it is very easy to think in an ellipse with a rotation and scale.

The inverse operation is also useful. For instance if you have a covariance matrix obtained experimentally and you wish to make a geometric interpretation of the distribution or just plot it, you can obtain the rotation matrix R and the scale vector l. Then the it is needed to compute the eigenvectors and eigenvalues. That is because:

R = eigvectors(\Sigma)
and
l = eigvalues(\Sigma)

Of course notice that if number of eigenVectors is lower than the dimensionality of the problem is because the ellipse is infinitely slim and as a consequence the determinant det \left | \Sigma \right | = 0 and then the matrix is singular. This means that one dimension should be removed if you want to compute likelihoods over that gaussian function.

Here I show how to represent a 3D gaussian function with ROS and rivz:

And here the code in python. Discussion about this subject in: http://answers.ros.org/question/2043/plot-a-gaussian-3d-representation-with-markers-in:

import roslib
roslib.load_manifest ("gaussian_markers")
import rospy
import PyKDL
from visualization_msgs.msg import Marker, MarkerArray
import visualization_msgs
from geometry_msgs.msg import Point
import numpy
from numpy import concatenate

#syntetic 3-D Gaussian probabilistic model for sampling
covMatModel =[[10, 10, 0],[10, 1, 0],[0, 0, 4]]
meanModel =[10, 10, 10]

#used to paint the autovectors
def markerVector(id,vector,position):
	marker = Marker ()
	marker.header.frame_id = "/root";
	marker.header.stamp = rospy.Time.now ()
	marker.ns = "my_namespace2";
	marker.id = id;
	marker.type = visualization_msgs.msg.Marker.ARROW
	marker.action = visualization_msgs.msg.Marker.ADD
	marker.scale.x=0.1
	marker.scale.y=0.3
	marker.scale.z=0.1
	marker.color.a= 1.0
	marker.color.r = 0.33*float(id)
	marker.color.g = 0.33*float(id)
	marker.color.b = 0.33*float(id)
	(start,end)=(Point(),Point())

	start.x = position[0]
	start.y = position[1]
	start.z = position[2]
	end.x=start.x+vector[0]
	end.y=start.y+vector[1]
	end.z=start.z+vector[2]

	marker.points.append(start)
	marker.points.append(end)
	print str(marker)
	return marker

rospy.init_node ('markersample', anonymous = True)
points_pub = rospy.Publisher ("visualization_markers", visualization_msgs.msg.Marker)
gauss_pub = rospy.Publisher ("gaussian", visualization_msgs.msg.Marker)

while not rospy.is_shutdown ():
	syntetic_samples = None

	#painting all the syntetic points
	for i in xrange (10, 5000):
		p = numpy.random.multivariate_normal (meanModel, covMatModel)
		if syntetic_samples == None:
			syntetic_samples =[p]
		else:
			syntetic_samples = concatenate ((syntetic_samples,[p]), axis = 0)

		marker = Marker ()
		marker.header.frame_id = "/root";
		marker.header.stamp = rospy.Time.now ()
		marker.ns = "my_namespace2";
		marker.id = i;
		marker.type = visualization_msgs.msg.Marker.SPHERE
		marker.action = visualization_msgs.msg.Marker.ADD
		marker.pose.position.x = p[0]
		marker.pose.position.y = p[1]
		marker.pose.position.z = p[2]
		marker.pose.orientation.x = 1
		marker.pose.orientation.y = 1
		marker.pose.orientation.z = 1
		marker.pose.orientation.w = 1
		marker.scale.x = 0.05
		marker.scale.y = 0.05
		marker.scale.z = 0.05
		marker.color.a = 1.0
		marker.color.r = 0.0
		marker.color.g = 0.0
		marker.color.b = 0.0
		points_pub.publish (marker)

	#calculating Gaussian parameters
	syntetic_samples = numpy.array (syntetic_samples)
	covMat = numpy.cov (numpy.transpose (syntetic_samples))
	mean = numpy.mean ([syntetic_samples[: , 0], syntetic_samples[: , 1], syntetic_samples[:, 2]], axis = 1)

	#painting the gaussian ellipsoid marker
	marker = Marker ()
	marker.header.frame_id ="/root";
	marker.header.stamp = rospy.Time.now ()
	marker.ns = "my_namespace";
	marker.id = 0;
	marker.type = visualization_msgs.msg.Marker.SPHERE
	marker.action = visualization_msgs.msg.Marker.ADD
	marker.pose.position.x = mean[0]
	marker.pose.position.y = mean[1]
	marker.pose.position.z = mean[2]

	#getting the distribution eigen vectors and values
	(eigValues,eigVectors) = numpy.linalg.eig (covMat)

	#painting the eigen vectors
	id=1
	for v in eigVectors:
		m=markerVector(id, v*eigValues[id-1], mean)
		id=id+1
		points_pub.publish(m)

	#building the rotation matrix
	eigx_n=PyKDL.Vector(eigVectors[0,0],eigVectors[0,1],eigVectors[0,2])
	eigy_n=-PyKDL.Vector(eigVectors[1,0],eigVectors[1,1],eigVectors[1,2])
	eigz_n=PyKDL.Vector(eigVectors[2,0],eigVectors[2,1],eigVectors[2,2])
	eigx_n.Normalize()
	eigy_n.Normalize()
	eigz_n.Normalize()
  	rot = PyKDL.Rotation (eigx_n,eigy_n,eigz_n)
	quat = rot.GetQuaternion ()

	#painting the Gaussian Ellipsoid Marker
	marker.pose.orientation.x =quat[0]
	marker.pose.orientation.y = quat[1]
	marker.pose.orientation.z = quat[2]
	marker.pose.orientation.w = quat[3]
	marker.scale.x = eigValues[0]*2
	marker.scale.y = eigValues[1]*2
	marker.scale.z =eigValues[2]*2

	marker.color.a = 0.5
	marker.color.r = 0.0
	marker.color.g = 1.0
	marker.color.b = 0.0

	gauss_pub.publish (marker)
	rospy.sleep (.5)

Libros para la introducción e investigación en Robótica

Como los que me conocéis sabéis, actualmente soy estudiante de Doctorado en el departamento ATC (Arquitectura y Tecnología de computadores) en E.T.S. Ingeniería Informática de Sevilla. La temática de mi tesis se ubica dentro del área de la robótica. Actualmente me encuentro en la fase de realización de la tesina (trabajo de investigación previo). Básicamente consiste en la realización de un estudio profundo del estado del arte , detectar carencias en las soluciones propuestas e introducir las líneas generales del trabajo de Tesis. Dentro de la robótica (por interés personal y por historia de la investigación en el departamento) me centro en las áreas de navegación, planificación, control compartido, sensores distribuidos y arquitecturas software robóticas.

Los artículos de investigación (Papers) son muy útiles para estar a la última, sin embargo la cantidad de papers que aparecen cada mes es desorbitarte, hay mucha información que no es relevante y en ocasiones es difícil discernir si las aportaciones son buenas, interesantes para tu investigación y merece la pena una profundización.

Sin embargo existe una fuente de información complementaria e igual o más importante que los papers: los libros. Los libros habitualmente están más atrasados que los papers, sin embargo, estos presentan innumerables cualidades: se profundiza mucho más en la temática, formato y notación unificada, aprendizaje progresivo, etc. Además, los libros ofrecen contenidos habitualmente más aceptados y reconocidos por la comunidad científica con una madurez media o alta. De esta forma se convierten en un pilar básico para la formación alrededor de los cuales debe girar el conocimiento adquirido en los artículos científicos (más efímeros y traicioneros). Aquí dejo una lista de los 4 libros (hay alguno más que me encantaron, pero estos son los principales) que para mí son sobresalientes y que están permitiéndome como estudiante de doctorado con vagos conocimientos previos en la robótica ir adquiriendo poco a poco destreza y experiencia como para poder aportar nuevas ideas.

Probabilistic Robotics – Año 2006. Un libro absolutamente imprescindible para comprender los enfoques probabilísticos que utiliza hoy día la robótica para aportar soluciones. Tiene una matemática dura, y todas las técnicas presentadas ofrecen su demostración matemática. En cualquier caso es un libro muy aplicable de manera casi directa en el software para robots. Se centra en las áreas de localización, construcción de mapas, modelos de sensores y modelos de movimiento de robots y planificación, todo desde un enfoque probabilístico. A pesar de explicar diversas técnicas exitosas en estas áreas el enfoque del libro no son estas sino más bien pretende ilustrar un punto de partida para enfocar los problemas robóticos de una forma mas realista y robusta haciendo uso de la estadística. Su principal autor Sebastian Thrun es hoy día una de las principales referencias mundiales en el área de la robótica.
Planning Algorithms – Año 2010. Este libro profundiza de forma exhaustiva en muchísimas técnicas y aspectos importantes en la planificación para robot. Se centra especialmente en la planificación de movimiento (aunque también trata temas de planificación de más alto nivel, tareas, etc). Es muy profundo, duro y formal matemáticamente. Más específicamente se centra en técnicas de planificación de movimiento global deterministas y basadas en muestras, planificación con restricciones kinodinámicas y no-holónomas, etc. Una de las principales ventajas de este libro es que está disponible online de forma gratuita, y además que es actualizado frecuentemente.
Springer Handbook of Robotics – La biblia de la robótica. Casi 1600 páginas de conocimiento. Bastante actualizado, finales del año 2008. Libro para ser utilizado como referencia para introducirse a cualquier temática de la robótica y encontrar fuentes. Trata de todos los temas: Fundamentos de la robótica (Cinemática, Dinámica, Mecánica, Control, Planificación, IA, Arquitecturas software), Estructuras de Robots, Sensores y percepción, manipulación, robótica distribuida, robótica de servicios, robótica en el ámbito de los seres humanos. El libro ha sido realizado por decenas de autores, especialistas en cada área. El nivel de formalismo y profundización varía según el capítulo y la madurez del tema. Otro libro muy recomendable para tener a mano.

Principles of Robot Motion – Este libro se centra en la planificación de movimiento. Es un libro cuya temática es muy parecida a Planning Algorithms, sin embargo es mucho mas ameno y sencillo de leer especialmente para los Ingenieros Informáticos (como yo) ya que presenta vocabulario y técnicas familiares del área de la IA. Realiza un estudio del arte (año 2005) en técnicas de planificación, centrándose en la planificación global (incluido sistemas con restricciones kinodinámicas). Toca también temas de cinemática, dinámica, técnicas de localización probabilísticas y SLAM.

Pues nada, ahí lo dejo, espero que pueda ayudar a cualquiera que esté iniciándose e investigando en este área. ¡Cualquier comentario, sugerencia u opiniones sobre estos u otros libros de robótica son bienvenidos!
Saludos. Bon apetit!

Testing Kinect and ROS

This afternoon I’ve been playing a bit with Kinect and ROS. The implemented software by openni and the ROS team is quite easy to use and the result are impressive. Our Group (RTC Robotic and Computer Technology from the Universidad de Sevilla) is interested in Kinect to bring to our robots (mainly for eldery care and rehabilitation proposes)the capability to understand better their environment: better navigation and SLAM, better Human-Robot Interaction, etc.

Here you can watch some basic samples:

Robótica: Introducción a ROS

El pasado Jueves 2 de Diciembre realizamos un seminario interno en el Departamento de Arquitectura y Tecnología de Computadores de introducción a la tecnología ROS.

Aquí dejo las slides por si a alguien les pudiera servir. Gracias por los contenidos Creative Commons de la web de ROS y de las transparencias creadas por “Radu Bogdan Rusu” para el evento “CoTeSys-ROS Fall School on Cognition-enabled Mobile Manipulation“. Este documento en sí es Creative Commons.


Saving Legacy Software: DLXView (reload) in Ubuntu 10.4 x64

DLXView is a software simulator which let you execute dlx-assembly code over different simulated processor architectures (among them, the famous static planning DLX proccessor, Tomasulo, etc.). This is  used manly for academic proposes by our department. Unfortunately this software quite old. It seems to be quite abandoned since its last modification was in 1997. AFAIK is now obsolete and unusable in several actual systems like Ubuntu 10.4 with x64.

Saving Legacy Software is always a challenge and it’s sometimes funny. This evening, after be struggling a bit with the source code, makefile and debugging with ddd I achieved make it run again properly on Ubuntu 10.4 in a x64 bit processor.

There were several problems:

  • Some pieces of code uses some deprecated system functionality related with error handling.
  • Some data types names overlapped others actual standard data structures.
  • Used libraries were quite old.

Hence in such situation building the code wasn’t possible. However some precompiled versions exist on the internet. They apparently worked fine in my system, but when an dlx-assembly file was loaded the program crashed. It was due to a buffer overflow in his code (specifically in the ReadFile function in the asm.c file). I’m not sure if this is related with the use of 64-bit processor or if it’s related with some changes of Tcl/Tk libraries for using unicode strings. Anyways it was solved making a  specific buffer bigger and using the function snprintf.

Of course these changes don’t assert that old or news problem appear. However it seems to be working fine, as it used to be.
Corrections and suggestions are welcome.

The old and official tarball
• The diff File:

diff -crB dlxview0.9/asm.c dlxview0.9b/asm.c
*** dlxview0.9/asm.c    1997-09-27 00:00:08.000000000 +0200
— dlxview0.9b/asm.c    2010-12-07 19:13:40.000000000 +0100
***************
*** 1368,1375 ****
register LoadInfo *infoPtr;    /* Information about the state of the
* assembly process. */
{
! #define MAX_LINE_SIZE 200
! #define MAX_NAME_SIZE 10
char line[MAX_LINE_SIZE];
char pseudoOp[MAX_NAME_SIZE+1];
FILE *f;
— 1368,1375 —-
register LoadInfo *infoPtr;    /* Information about the state of the
* assembly process. */
{
! #define MAX_LINE_SIZE 2000
! #define MAX_NAME_SIZE 100
char line[MAX_LINE_SIZE];
char pseudoOp[MAX_NAME_SIZE+1];
FILE *f;
***************
*** 1377,1383 ****
int i, nullTerm;
char *end, *curToken;
char savedChar;
!     char msg[40];
double strtod();

f = fopen(fileName, “r”);
— 1377,1383 —-
int i, nullTerm;
char *end, *curToken;
char savedChar;
!     char msg[255];
double strtod();

f = fopen(fileName, “r”);
Only in dlxview0.9: dlxview
diff -crB dlxview0.9/Makefile dlxview0.9b/Makefile
*** dlxview0.9/Makefile    1997-09-27 00:00:08.000000000 +0200
— dlxview0.9b/Makefile    2010-12-07 19:00:28.000000000 +0100
***************
*** 1,8 ****
# environment dependent variables; change these to their appropriate
# directory paths for the X11 and tcl/tk library and header paths
# Solaris paths
! TCL_LIB = /usr/local/lib
! TCL_INC = /usr/local/include
X_LIB = /opt/x11r5/lib
X_INC = /usr/local/X11R5/include
# Linux paths
— 1,8 —-
# environment dependent variables; change these to their appropriate
# directory paths for the X11 and tcl/tk library and header paths
# Solaris paths
! TCL_LIB = /usr/lib
! TCL_INC = /usr/include/tcl
X_LIB = /opt/x11r5/lib
X_INC = /usr/local/X11R5/include
# Linux paths
***************
*** 20,26 ****

# Solaris may need the -lsocket and -lnsl flags added.  Tk4.1 and newer
# also require the -ldl flag.
! LLIBS = ${TCL_LIB}/libtcl7.6.a ${TCL_LIB}/libtk4.2.a -L${X_LIB} -lX11 -lnsl -lsocket -lm -ldl
# LLIBS = ${TCL_LIB}/libtcl.so ${TCL_LIB}/libtk.so -L${X_LIB} -lX11 -lm -ldl
# LLIBS = ${TCL_LIB}/libtcl.a ${TCL_LIB}/libtk.a -L${X_LIB} -lX11 -lm

— 20,26 —-

# Solaris may need the -lsocket and -lnsl flags added.  Tk4.1 and newer
# also require the -ldl flag.
! LLIBS = -g ${TCL_LIB}/libtcl.a ${TCL_LIB}/libtk.a ${TCL_LIB}/libpthread.so ${TCL_LIB}/libc.so -L${X_LIB} -lX11 -lnsl -lm -ldl
# LLIBS = ${TCL_LIB}/libtcl.so ${TCL_LIB}/libtk.so -L${X_LIB} -lX11 -lm -ldl
# LLIBS = ${TCL_LIB}/libtcl.a ${TCL_LIB}/libtk.a -L${X_LIB} -lX11 -lm

***************
*** 30,36 ****
###########################################################################

CC = gcc
! CFLAGS = ${CPPFLAGS} -O -I. -I${TCL_INC} -I${X_INC}
TARGET = dlxview

OBJS = asm.o cop0.o getput.o io.o issue.o main.o script.o sim.o stop.o sym.o trap.o
— 30,36 —-
###########################################################################

CC = gcc
! CFLAGS = ${CPPFLAGS} -O -I. -I${TCL_INC} -I${X_INC} -g
TARGET = dlxview

OBJS = asm.o cop0.o getput.o io.o issue.o main.o script.o sim.o stop.o sym.o trap.o
Only in dlxview0.9: .project
diff -crB dlxview0.9/script.h dlxview0.9b/script.h
*** dlxview0.9/script.h    1997-09-27 00:00:08.000000000 +0200
— dlxview0.9b/script.h    2010-12-07 17:16:15.000000000 +0100
***************
*** 927,934 ****
set help_cmd_stack \”HelpGeneral\”\n\
$t delete 1.0 end\n\
HelpIn help_title {\n\
!                        DLXview: Interactive Animated DLX Pipeline
!                Visualization\n\
}\n\
HelpIn HelpInfo {\n\
1. General Information\n\
— 927,933 —-
set help_cmd_stack \”HelpGeneral\”\n\
$t delete 1.0 end\n\
HelpIn help_title {\n\
!                        DLXview: Interactive Animated DLX Pipeline Visualization\n\
}\n\
HelpIn HelpInfo {\n\
1. General Information\n\
diff -crB dlxview0.9/sim.c dlxview0.9b/sim.c
*** dlxview0.9/sim.c    1997-09-27 00:00:08.000000000 +0200
— dlxview0.9b/sim.c    2010-12-07 17:30:43.000000000 +0100
***************
*** 27,32 ****
— 27,33 —-
#include <string.h>
#include <stdlib.h>
#include <tcl.h>
+ #include <errno.h>
#include “dlx.h”
#include “sym.h”

***************
*** 1033,1049 ****
static char *
errstring(void)
{
!     extern int errno, sys_nerr;
!     extern char *sys_errlist[];
static char msgbuf[64];

if( !errno )
return “no error”;
!     if( errno >= sys_nerr ) {
sprintf( msgbuf, “unknown error %d”, errno );
return msgbuf;
}
!     return sys_errlist[ errno ];
}

— 1034,1051 —-
static char *
errstring(void)
{
!     extern int errno;
!     char * strerror_msg;
static char msgbuf[64];

+     strerror_msg=strerror( errno);
if( !errno )
return “no error”;
!     if( strerror_msg == 0 ) {
sprintf( msgbuf, “unknown error %d”, errno );
return msgbuf;
}
!     return strerror_msg ;
}

Robotics – Simultaneous Localization And Mapping (SLAM) con ROS

Una tarea muy importante que debe realizar un robot autónomo es adivinar donde está localizado dado un mapa conocido, en  la bibliografía este problema es referido como “el problema de la localización”. Las cosas se complican cuando el mapa también es desconocido: Esta situación se conoce como “The kidnaped robot problem”. En estos casos hay que utilizar técnicas de SLAM (Localización y Mapeo Simultaneo) para ambos: localizarse y construir el mapa del entorno simultáneamente. De forma laxa podríamos decir que se trata de un tipo de exploración del robot en un entorno desconocido. Existen muchas y diversas técnicas de SLAM típicamente para 2D y 3D y utilizando sensores Lasers Rangers, Sonars o Cámaras mono o estéreo.

ROS dispone de varios paquetes para realizar SLAM usando visión  con cámaras y principalmente usando Lasers Rangers: vslam, gmapping, kartoMapping, graphmapping.

Este post trataremos como arrancar un ejemplo básico con gmapping, El objetivo es aprender vagamente como funciona y poder aplicarlo en nuestros proyectos.

El algoritmo GMapping

GMapping es una librería libre perteneciente al proyecto OpenSLAM y que implementa una extensión la técnica SLAM Grid Mapping. En cualquier caso, utilizaremos este software como si fuera una caja negra. Sabemos que su salida son la localización del robot en el mapa y el mapa en si mismo. Como entradas toma  información periódica sobre una estimación de la posición del robot (odometría – calculada por los encoders del motor) y la medida del sensor laser ranger (vector de distancia) en un entorno con superficie plana.

Demo GMapping en ROS

¿Como probar este software en ROS? Sencillo, la gente de Bosch ya ha realizado una demo, con el simulador stage para que podamos abordar el problema. Esta demo está localizado en el paquete explore_stage. Consiste en un mapa desconocido por un robot móvil tipo erratic. Observemos la wiki del paquete vemos la siguiente información de repositorio donde se ubica el código:

Descargaremos todo el directorio […]stacks/exploration/demos ya que contiene algunos paquetes adicionales con mapas y modelos que nos serán necesarios.

geus@geus-vaio:/opt/ros/cturtle/stacks/external/demos$ svn co https://bosch-ros-pkg.svn.sourceforge.net/svnroot/bosch-ros-pkg/trunk/stacks/exploration
[…]
geus@geus-vaio:/opt/ros/cturtle/stacks/external/demos/exploration$ roscd explore_stage/
geus@geus-vaio:/opt/ros/cturtle/stacks/external/demos/exploration/explore_stage$ ls
config   explore.launch       explore_slam.xml  explore.xml   move.xml
explore  explore_slam.launch  explore.vcg       manifest.xml

Como se observa el paquete es muy sencillo, solo algunos componentes .launch con la aplicaciones.  Antes que nada compilemos el stack exploration ya que contiene un paquete exploration/explore con el código para realizar la exploración en el mapa desconocido.

geus@geus-vaio:/opt/ros/cturtle/stacks/external/demos/exploration/explore$ rosmake exploration
[ rosmake ] Packages requested are: [‘exploration’]
[…]
[ rosmake ] Results:
[ rosmake ] Built 53 packages with 0 failures.

En concreto la aplicación de explorar está en el explore_slam.launch. Lanzamos la aplicación:

geus@geus-vaio:/opt/ros/cturtle/stacks/external/demos/exploration/explore_stage$ roslaunch explore_slam.launch

Algunos problemas con Stage

Pueden que los usuarios de ubuntu reciban el siguiente error, debido a los cambios en la política de archivos del servidor gráfico X11. Existe un problema que no se encuentra el archivo rgb.txt que es una base de datos con colores.

[Loading /opt/ros/cturtle/stacks/external/full_organization_repositories/bosh-ros-pkg/bosch_demos/bosch_worlds/maze-noisy.world][Include segway-noisy.inc][Include sick.inc][ INFO] [1291307466.316198795]: Subscribed to Topics: base_scan
err: unable to open color database: No such file or directory (try adding rgb.txt’s location to your STAGEPATH) (/tmp/buildd/ros-cturtle-simulator-stage-1.2.1/debian/ros-cturtle-simulator-stage/opt/ros/cturtle/stacks/simulator_stage/stage/build/Stage-3.2.2-Source/libstage/color.cc Color)

El problema se soluciona de forma sencilla añadiendo a la variable de entorno $STAGEPATH una ruta que contenga un rgb.txt correcto. En nuestro caso, el mismo paquete stage contiene ese archivo. Lo añadimos:

geus@geus-vaio:/opt/ros/cturtle/stacks$ find $ROS_PACKAGE_PATH | grep rgb.txt
/opt/ros/cturtle/stacks/simulator_stage/stage/share/stage/assets/rgb.txt
/opt/ros/cturtle/stacks/simulator_stage/stage/share/stage/rgb.txt
/opt/ros/cturtle/stacks/simulator_stage/stage/build/Stage-3.2.2-Source/assets/rgb.txt
/opt/ros/cturtle/stacks/simulator_stage/stage/build/Stage-3.2.2-Source/libstage/rgb.txt
geus@geus-vaio:/opt/ros/cturtle/stacks$ export STAGEPATH=`rospack find stage`/share/stage
geus@geus-vaio:/opt/ros/cturtle/stacks$ echo $STAGEPATH
/opt/ros/cturtle/stacks/simulator_stage/stage/share/

Ahora si. Lanzamos y ya funciona y podemos ver como arrancan varios procesos definidos en el archivo .launch.

geus@geus-vaio:/opt/ros/cturtle/stacks/external/demos/exploration/explore_stage$ roslaunch ./explore_slam.launch

Otra alternativa es añadir en el archivo explore_slam.launch el valor de la variable de entorno:



...

Interpretación de resultados

Tras lanzar el explore_slam.launch se ejecutan diversos procesos ROS. Entre ellos se abre el simulador stage, mostrado en la imagen. A los pocos segundos, tras algo de proceso el robot empieza a navegar y a explorar su entorno. Para poder ver bien que está ocurriendo podemos abrir la herramienta rviz. Primera imagen de este artículo.

geus@geus-vaio:/opt/ros/cturtle/stacks$ rosrun rviz rviz

Añadimos los visualizadores (plugins) de rviz “map”, “tf” y “laser scan”.

  • “map” – sirve para que podamos ver el mapa que se está construyendo.  Mas concretamente monitorizar los mensajes de tipo nav_msgs/OccupancyGrid publicados en el topic “/map”.
  • “tf” – sirve para monitorizar la posición estimada por el sistema SLAM.
  • “laser scan”- sirve para monitorizar el escaneo laser que está realizando el robot. Mas concretamente mensajes del tipo sensor_msgs/LaserScan

Podemos observar que se han creado tres procesos principales: “/explore” “/move_base” “/stage” “/gmaping”.

  • “/stage”- es el nodo simulador. Se limita a enviar al exterior mensajes de las lecturas del laser y la posición del robot a través del tópic /tf. También acepta comandos de movimiento a través del topic /cmd_vel
  • “/gmaping”- es el nodo que crea que realiza el SLAM. Crea el mapa a partir de la posición estimada y las lecturas del sensor Laser (topic /base_scan de tipo “sensor_msgs/LaserScan”).
  • “/explore” – es el nodo que realiza el algoritmo de exploración. Utiliza el mapa resultante y la posición del robot para estimar zonas donde es mas interesante seguir explorando. Una vez decidido un plan de exploración se lo indica al nodo “/move_base”
  • “/move_base” – es el nodo que se encarga de ejecutar un plan de ejecución. Se trata de un paquete esencial y genérico que ROS provee. Simplente sigue las ordenes del nodo “/explore” y ordena al “/stage” como debe mover los motores haciendo uso del topic “/cmd_vel” de tipo de mensaje “geometry_msgs/Twist
    “, siempre intentando crear una trayectoria sin colisionar.

Esto es todo. Si lográis sustituir el nodo stage por un robot real con odometría y sensor laser, tenéis un sistema de SLAM listo para ser ejecutado.

Saludos y Happy Robotics! 😉