Robotics StackExchange | Archived questions

Converting sensors_msgs/NavSatFix to nmea_msgs/Sentence messages

Hello,

I need to convert NavSatFix messages to nmea_msgs/Sentence messages. What's the best way of doing that? Are there any packages I can use for that purpose?

Asked by rythm07 on 2021-05-06 10:29:46 UTC

Comments

Answers

I have not found a package that can do this already, but here is some code to start off of if you are interested in rolling your own. Note that it was made for a GPSFix message instead of a NavSatFix, but the fields should be similar. Note some of the GPGGA fields are hard coded (such as altitude) so it may require some modification. If you want other messages than GPGGA then you would need to parse and populate those as well (see a NMEA reference for message structure).

Hope this helps.

void GPSRx(const gps_common::GPSFix::ConstPtr &msg)
{
  char buf[255]; // Buffer for GPGGA sentence

  // Time conversion
  uint32_t hours = msg->time / 3600;
  uint32_t minutes = (msg->time - (3600 * hours)) / 60;
  float seconds = msg->time - (float)(3600 * hours) - (float)(60 * minutes);

  // Latitude conversion
  char lat_dir;
  int8_t lat_degs = msg->latitude;
  float lat_mins = (msg->latitude - (float)lat_degs) * 60.0;

  if(lat_degs < 0 )
  {
    lat_degs *= -1;
    lat_mins *= -1;
    lat_dir = 'S';
  }
  else
  {
    lat_dir = 'N';
  }

  // Longitude conversion
  char lon_dir;
  int8_t lon_degs = msg->longitude;
  float lon_mins = (msg->longitude - (float)lon_degs) * 60.0;

  if(lon_degs < 0 )
  {
    lon_degs *= -1;
    lon_mins *= -1;
    lon_dir = 'W';
  }
  else
  {
    lon_dir = 'E';
  }

  // Populate a GPGGA sentence
  uint8_t len = sprintf(buf, "$GPGGA,%02d%02d%04.1f,%02d%018.15f,%c,%02d%018.15f,%c,1,%d,0.9,50.4,M,46.9,M,,",
                        hours,
                        minutes,
                        seconds,
                        lat_degs,
                        lat_mins,
                        lat_dir,
                        lon_degs,
                        lon_mins,
                        lon_dir,
                        msg->status.satellites_visible);

  // Calculate checksum of sentence and add it to the end of the sentence
  uint8_t checksum = 0;
  for(int i = 1; i < len; i++)
  {
    checksum ^= buf[i];
  }
  sprintf(&buf[len], "*%02X",checksum);

  // Put the sentence in a ros nmea_msgs/Sentence message and publish
  nmea_msgs::Sentence nmea;
  nmea.sentence = buf;
  nmea_pub_.publish(nmea);
}

Asked by djchopp on 2021-05-07 08:47:23 UTC

Comments

Thank you so much the help! It will definitely give me something to start with!

Asked by rythm07 on 2021-05-07 10:26:24 UTC