ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Geotag image - OpenCV-Mavros

asked 2019-09-07 09:46:10 -0500

elgarbe gravatar image

Hi, I have a drone with a pixhawk flight controller ans a raspberry pi as a companion computer. Pixhawk publish through mavros GPS information. I have a web cam connected to raspbery pi too. So I would like to write a ROS node that take a picture every one second and write exif data from mavros. I work with python. I need a little help to start coding an to choose a way to manage exif data.

Right now I have the usb_cam publishing image at 1FPS and a node that subscribe to it, using openCV I manage to get the image and save it on a file. geotag data is the part that I need some help.


edit retag flag offensive close merge delete


Have you managed to find any suitable solution? I'm looking at the same problem... I have been thinking into saving the location data to a file at the same time the picture is taken and trying to pós processing it latter.

aureliokta gravatar image aureliokta  ( 2019-10-20 10:06:50 -0500 )edit

1 Answer

Sort by » oldest newest most voted

answered 2019-10-20 16:41:43 -0500

elgarbe gravatar image

Hi, I've managed to make it work with this code:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
from __future__ import print_function

import message_filters
import os
import sys
import rospy
import cv2

from PIL import Image
import piexif

from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import NavSatFix

from fractions import Fraction
from datetime import datetime

# Constants that may change in multiple places

class mapping_hexa:

  def __init__(self):
    self.bridge = CvBridge()

    # Me suscribo al tópico donde se envian las imágenes
    self.image_sub = message_filters.Subscriber("/usb_cam/image_raw", Image)
    # Me suscribo al tópico donde se envian los datos de odometría
    self.gps_sub   = message_filters.Subscriber("/mavros/global_position/global", NavSatFix)

    # Creo el objeto que sincroniza los mensajes recibidos
    self.synchronizer = message_filters.ApproximateTimeSynchronizer(
            [self.image_sub, self.gps_sub],

  def rostime2floatSecs(self, rostime):
    return rostime.secs + (rostime.nsecs / 1000000000.0)

  def to_deg(self, value, loc):
    """convert decimal coordinates into degrees, munutes and seconds tuple
    Keyword arguments: value is float gps-value, loc is direction list ["S", "N"] or ["W", "E"]
    return: tuple like (25, 13, 48.343 ,'N')
    if value < 0:
      loc_value = loc[0]
    elif value > 0:
      loc_value = loc[1]
      loc_value = ""
    abs_value = abs(value)
    deg = int(abs_value)
    t1 = (abs_value - deg) * 60
    min = int(t1)
    sec = round((t1 - min) * 60, 5)
    return (deg, min, sec, loc_value)

  def change_to_rational(self, number):
    """convert a number to rantional
    Keyword arguments: number
    return: tuple like (1, 2), (numerator, denominator)
    f = Fraction(str(number))
    return (f.numerator, f.denominator)

  def set_gps_location(self, file_name, lat, lng, altitude, gpsTime):
    """Adds GPS position as EXIF metadata
    Keyword arguments:
    file_name -- image file
    lat -- latitude (as float)
    lng -- longitude (as float)
    altitude -- altitude (as float)
    lat_deg = self.to_deg(lat, ["S", "N"])
    lng_deg = self.to_deg(lng, ["W", "E"])

    exiv_lat = (self.change_to_rational(lat_deg[0]), self.change_to_rational(lat_deg[1]), self.change_to_rational(lat_deg[2]))
    exiv_lng = (self.change_to_rational(lng_deg[0]), self.change_to_rational(lng_deg[1]), self.change_to_rational(lng_deg[2]))

    gps_ifd = {
      piexif.GPSIFD.GPSVersionID: (2, 0, 0, 0),
      piexif.GPSIFD.GPSAltitudeRef: 0,
      piexif.GPSIFD.GPSAltitude: self.change_to_rational(round(altitude)),
      piexif.GPSIFD.GPSLatitudeRef: lat_deg[3],
      piexif.GPSIFD.GPSLatitude: exiv_lat,
      piexif.GPSIFD.GPSLongitudeRef: lng_deg[3],
      piexif.GPSIFD.GPSLongitude: exiv_lng,
      piexif.GPSIFD.GPSDateStamp: gpsTime

    exif_dict = {"GPS": gps_ifd}
    exif_bytes = piexif.dump(exif_dict)
    piexif.insert(exif_bytes, file_name)

  def make_image(self, img_data, gps_data):
    # log some info about the image topic
    rospy.loginfo("img time_ %i", img_data.header.stamp.secs)
    rospy.loginfo("gps time: %i", gps_data.header.stamp.secs)

      cv_img = self.bridge.imgmsg_to_cv2(img_data, "bgr8")
    except CvBridgeError as e:

    output_dir = "../img/"
    jpg_quality = 100
    compr = cv2.IMWRITE_JPEG_QUALITY;
    quality = jpg_quality  # jpg quality is in [0,100] range, png [0,9]
    params = [compr, quality]

    picGpsTimeFlt = self.rostime2floatSecs(gps_data.header.stamp)
    picNameTimeString = datetime.utcfromtimestamp(picGpsTimeFlt).strftime('%Y-%m-%dT%H:%M:%S.%fZ')
    picSaveName = ""
    picSaveName = picNameTimeString + ".jpg"
    print("save image ")
    picPath = os.path.join(output_dir, picSaveName)
    cv2.imwrite(picPath, cv_img, params)

    picGpsTimeFlt =self.rostime2floatSecs(gps_data.header.stamp)
    gpsTimeString = datetime.utcfromtimestamp(picGpsTimeFlt).strftime('%Y:%m:%d %H:%M:%S')
    self.set_gps_location(picPath, lat ...
edit flag offensive delete link more

Question Tools

1 follower


Asked: 2019-09-07 09:46:10 -0500

Seen: 980 times

Last updated: Oct 20 '19