ROS GPS with tinygps and rosserial

Exploração do sistema GPS no ROS.

Esta página reúne alguma informação recolhida na minha pesquisa e experiências com os dispositivo de localização GPS uBlox neo 6M e neo M8N no ROS.

uBlox neo-6M chip in one of the used modules

As experiências foram efectuadas num ESP32 de 38 pinos, com a ligação serial no RX2 e TX2, e alimentação de 3.3V.

ESP32 – 38 pins pinout

A exploração das informações recolhidas do GPS foi efectuada com a biblioteca tinygpsplus.

http://arduiniana.org/libraries/tinygpsplus/

A programação necessária foi efectuada no vscode com o platformio.

NMEA sentences

As informações go GPS são obtidas com base em fluxos de texto cujas linhas começam com as seguintes sequências de caracteres e cada uma delas agrupa informações especificas.

$GPRMC
$GPVTG
$GPGGA
$GPGSA
$GPGSV
$GPGLL

No caso do neo6M verifiquei que existem 3 linhas seguidas da $GPGSV

https://web.fe.up.pt/~ee95080/NMEA%20data.pdf

https://www.trimble.com/oem_receiverhelp/v4.44/en/NMEA-0183messages_MessageOverview.html

 

ESP32 tinygps rosserial

Este repositório contem o software que publica mensagens GPS num tópico do ROS.

https://github.com/inaciose/esp32_ard_rosserial_gps

Neste momento o tipo de mensagem publicada é a GPSFix, por ser a mais completa.

Assim que possível serão feitas as alterações necessitarias para poder escolher entre mensagens GPSFix e NavSatFix.

ROS messages

Existem dois tipos de mensagens usados na publicação das informações do GPS:

  • NavSatFix
  • GPSFix

O pacote gps_common contem um script para publicar conversões entre estes dois tipos de mensagens.

https://github.com/swri-robotics/gps_umd/blob/master/gps_common/nodes/fix_translator

Mensagens NavSatFix

Estas mensagens fazem parte do grupo das sensor_msgs e incluem, para além da longitude, latitude e altitude, informações do tipo NavSatStatus (que contẽm informaçoes sobre os satélites)

Este tipo de mensagens contêm menos informações que as do tipo GPSFix.

  • https://docs.ros.org/en/api/sensor_msgs/html/msg/NavSatFix.html
  • https://docs.ros.org/en/api/sensor_msgs/html/msg/NavSatStatus.html

Mensagens GPSFix

Estas mensagens fazem parte do grupo das gps_common, definidas no pacote com o mesmo nome e incluem mais informações que as mensagens do tipo NavSatFix, também incluem informações do tipo GPSStatus (que contẽm informaçoes sobre os satélites)

  • http://docs.ros.org/en/api/gps_common/html/msg/GPSFix.html
  • http://docs.ros.org/en/api/gps_common/html/msg/GPSStatus.html

Algumas particularidades na publicação da mensagem GPSFix

Devido ao limite do  buffer do rosserial, o numero de satélites visíveis sobre os quais é publicada informação está limitado a 8 . Neste momento não estão a ser seleccionados todos os satélites em uso, o limite é por ordem no array.

A mensagem de erro na consola do ROS é a seguinte:

[ERROR] [1631463315.015253]: Message from device dropped: message larger than buffer.

Pelo que percebi, os satélites são aqueles cujo SNR é zero.

Outra particularidade é como preencher campos de mensagens ros que são arrays variáveis.

Segundo as informações no ros wiki (http://wiki.ros.org/msg) os arrais de inteiros são preenchidos com variáveis do tipo std::vector<T>, mas no rosserial este tipo não está disponível.

Para contornar este problema devemos, fazer com que o campo aponte para um array pré declarado, e posteriormente  atribuir o numero de itens no array a um campo não documentado (msg.campo_lenght) na definição do tipo da msg do ros.

int array[MAX_ITENS];
(put values in the array)
gps_status_msg.satellite_visible_prn = array;
gps_status_msg.satellite_visible_prn_length = array_item_count;

GPSFix – Informações não preenchidas ou por confirmar

# Direction (degrees from north)
float64 track – gps.course.deg() ??? is from north?

# Vertical speed (meters/second)
float64 climb

# Device orientation (units in degrees)
float64 pitch
float64 roll
float64 dip

# Total (positional-temporal) dilution of precision
float64 gdop

# Temporal dilution of precision
float64 tdop

# Uncertainty of measurement, 95% confidence
all err_ and covariance information

GPSStatus – Informações não preenchidas

# Satellites used in solution
int32[] satellite_used_prn # PRN identifiers

uint16 motion_source # Source for speed, climb and track
uint16 orientation_source # Source for device orientation
uint16 position_source # Source for position

 

ROS packages

gps_umd

https://wiki.ros.org/gps_umd (meta package)

https://github.com/swri-robotics/gps_umd

  • gps_common
  • gpsd_client

gpsd_viewer

http://wiki.ros.org/gpsd_viewer

nmea_navsat_driver

http://wiki.ros.org/nmea_navsat_driver

https://github.com/ros-drivers/nmea_navsat_driver

nmea_msgs

http://wiki.ros.org/nmea_msgs

https://github.com/ros-drivers/nmea_msgs

 

Links

https://answers.ros.org/question/327193/how-have-a-static-map-to-understand-its-gps-coordinates/

https://answers.ros.org/question/327193/how-have-a-static-map-to-understand-its-gps-coordinates/

https://answers.ros.org/question/220368/using-mapviz-gpsfix-vs-navsatfix/

https://medium.com/@hitlx916/visualize-the-gnss-messages-in-mapviz-ros-4ae7eec19936

http://wiki.ros.org/mapviz

https://github.com/nobleo/rviz_satellite