From 0ad8e3893a58677f2600f3421afcb9891022d9bf Mon Sep 17 00:00:00 2001
From: "Pasch, Frederik" <frederik.pasch@intel.com>
Date: Tue, 22 Jan 2019 18:22:13 +0100
Subject: [PATCH] Add handling of a GNSS Sensor

---
 src/carla_ros_bridge/gnss.py   | 84 ++++++++++++++++++++++++++++++++++
 src/carla_ros_bridge/sensor.py |  3 ++
 2 files changed, 87 insertions(+)
 create mode 100644 src/carla_ros_bridge/gnss.py

diff --git a/src/carla_ros_bridge/gnss.py b/src/carla_ros_bridge/gnss.py
new file mode 100644
index 00000000..f9142aef
--- /dev/null
+++ b/src/carla_ros_bridge/gnss.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+
+#
+# Copyright (c) 2018 Intel Labs.
+#
+# authors: Frederik Pasch (frederik.pasch@intel.com)
+#
+"""
+Classes to handle Carla gnsss
+"""
+
+import numpy
+
+import tf
+
+from sensor_msgs.msg import NavSatFix
+
+from carla_ros_bridge.sensor import Sensor
+import carla_ros_bridge.transforms as trans
+
+
+class Gnss(Sensor):
+
+    """
+    Actor implementation details for gnss sensor
+    """
+
+    def __init__(self, carla_actor, parent, topic_prefix=None, append_role_name_topic_postfix=True):
+        """
+        Constructor
+
+        :param carla_actor: carla actor object
+        :type carla_actor: carla.Actor
+        :param parent: the parent of this
+        :type parent: carla_ros_bridge.Parent
+        :param topic_prefix: the topic prefix to be used for this actor
+        :type topic_prefix: string
+        :param append_role_name_topic_postfix: if this flag is set True,
+            the role_name of the actor is used as topic postfix
+        :type append_role_name_topic_postfix: boolean
+        """
+        if topic_prefix is None:
+            topic_prefix = 'gnss'
+        super(Gnss, self).__init__(carla_actor=carla_actor,
+                                   parent=parent,
+                                   topic_prefix=topic_prefix,
+                                   append_role_name_topic_postfix=append_role_name_topic_postfix)
+
+    def get_tf_msg(self):
+        """
+        Function (override) to modify the tf messages sent by this gnss.
+
+        The gnss transformation has to be altered:
+        for some reasons gnss sends already a rotated cloud,
+        so herein, we need to ignore pitch and roll
+
+        :return: the filled tf message
+        :rtype: geometry_msgs.msg.TransformStamped
+        """
+        tf_msg = super(Gnss, self).get_tf_msg()
+        rotation = tf_msg.transform.rotation
+        quat = [rotation.x, rotation.y, rotation.z, rotation.w]
+        dummy_roll, dummy_pitch, yaw = tf.transformations.euler_from_quaternion(
+            quat)
+        # set roll and pitch to zero
+        quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
+        tf_msg.transform.rotation = trans.numpy_quaternion_to_ros_quaternion(
+            quat)
+        return tf_msg
+
+    def sensor_data_updated(self, carla_gnss_event):
+        """
+        Function to transform a received gnss event into a ROS NavSatFix message
+
+        :param carla_gnss_event: carla gnss event object
+        :type carla_gnss_event: carla.GnssEvent
+        """
+        navsatfix_msg = NavSatFix()
+        navsatfix_msg.header = self.get_msg_header(use_parent_frame=False)
+        navsatfix_msg.latitude = carla_gnss_event.latitude
+        navsatfix_msg.longitude = carla_gnss_event.longitude
+        navsatfix_msg.altitude = carla_gnss_event.altitude
+        self.publish_ros_message(
+            self.topic_name() + "/gnss", navsatfix_msg)
diff --git a/src/carla_ros_bridge/sensor.py b/src/carla_ros_bridge/sensor.py
index 19579d3b..06de9ff2 100644
--- a/src/carla_ros_bridge/sensor.py
+++ b/src/carla_ros_bridge/sensor.py
@@ -43,6 +43,8 @@ def create_actor(carla_actor, parent):
             return Camera.create_actor(carla_actor=carla_actor, parent=parent)
         if carla_actor.type_id.startswith("sensor.lidar"):
             return Lidar(carla_actor=carla_actor, parent=parent)
+        if carla_actor.type_id.startswith("sensor.other.gnss"):
+            return Gnss(carla_actor=carla_actor, parent=parent)
         else:
             return Sensor(carla_actor=carla_actor, parent=parent)
 
@@ -166,3 +168,4 @@ def sensor_data_updated(self, carla_sensor_data):
 # these imports have to be at the end to resolve cyclic dependency
 from carla_ros_bridge.camera import Camera  # pylint: disable=wrong-import-position
 from carla_ros_bridge.lidar import Lidar   # pylint: disable=wrong-import-position
+from carla_ros_bridge.gnss import Gnss   # pylint: disable=wrong-import-position