Posición ground truth y mapa de segmentos
Esta semana estuve trabajando en obtener una pose ground truth y desarrollando el nodo que hace el mapa de segmentos.
Ground truth pose
Para simplificar el problema, hice borrón y cuenta nueva y planteé un nuevo enfoque. En vez de centrarme en que no exista derrape de las ruedas del robot, simplifiqué haciendo un nodo que obtenga la pose verdadera a partir de la pose dada por Gazebo. El plugin de Gazebo utilizado para ello es PosePublisher. Por ejemplo, para la pose leída en Gazebo x = 0.41, y = 0.89, yaw = 1.62 rad obtenemos la pose:
header: stamp: sec: 0 nanosec: 0 frame_id: odom pose: position: x: 0.40888364009520894 y: 0.8904541949567006 z: 0.0016751992690483544 orientation: x: 0.0007838543727473632 y: -0.0010215381742038803 z: 0.7245271386625971 w: 0.6892450706196386
Si el cuaternión lo pasamos a radianes obtenemos un yaw de 1.62 radianes, luego el nodo da la pose correctamente.
Nodo para hacer mapa de segmentos
Teniendo la pose ground truth y los datos de LaserScan, trabajamos en un nodo que permitiera generar un mapa de segmentos. Los pasos que sigue para ello son:
- Obtener puntos 2D en el frame del láser.
- Obtener puntos en base_link y de ahí en map usando la pose ground truth.
- Se crean los segmentos en el frame map con el algoritmo RANSAC descrito en los posts anteriores
En el siguiente vídeo se puede ver el funcionamiento. Actualmente estamos trabajando en mejorar la fusión de segmentos similares.
