Merge pull request #58 from uci-uav-forge/52-telem-subscriber

52 telem subscriber

CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   std_msgs
   visualization_msgs
+  message_filters
 )
 
 ## System dependencies are found with CMake's conventions

interop

@@ -10,9 +10,20 @@
 
 
 from google.protobuf import json_format
+import threading
+import json
+
 from auvsi_suas.client import client
-import rospy, threading
-from std_msgs.msg import Header
+from auvsi_suas.proto import interop_api_pb2
+
+import rospy
+import message_filters
+
+from std_msgs.msg import (
+    Header,
+    Float64
+)
+from sensor_msgs.msg import NavSatFix
 from geographic_msgs.msg import (
     GeoPoint,
 )
@@ -25,7 +36,6 @@ from uavfros.msg import (
     Waypoints,
     StationaryObstacles,
 )
-
 from uavfros.srv import MissionService
 
 # AUVSI SERVER INFO
@@ -61,6 +71,9 @@ class UAVFInteropClient(client.AsyncClient):
         mission_dict = json_format.MessageToDict(mission.result())
         return mission_dict
 
+    def post_telemtry(self, telem):
+        self.post('/api/telemetry', data=telem)
+
 
 def pack_mission_to_msg(mission: dict):
     msg = Mission()
@@ -170,6 +183,21 @@ def mission_server():
     mission_serv = rospy.Service(MISSION_SERVER_TOPIC, MissionService, handle_mission)
     rospy.spin()
 
+def telem_subscriber():
+    global_sub = message_filters.Subscriber("/mavros/global_position/global", NavSatFix)
+    hdg_sub = message_filters.Subscriber("/mavros/global_position/compass_hdg", Float64)
+    ts = message_filters.ApproximateTimeSynchronizer([global_sub, hdg_sub], 10, 0.1)
+    ts.registerCallback(send_telemetry)
+    rospy.spin()
+
+def send_telemetry(global_data, hdg_data):
+    telem_dict = {'latitude': global_data.latitude,
+                  'longitude': global_data.longitude,
+                  'altitude': global_data.altitude,
+                  'heading': hdg_data.data}
+    telem_json = json.dumps(telem_dict)
+    interopclient.post_telemtry(telem_json)
+
 
 # def odlc_callback(data):
 #     client.upload_odlc(data)
@@ -179,10 +207,6 @@ def mission_server():
 #     client.upload_map(1, data)
 
 
-# def telemetry_callback(data):
-#     client.upload_telemetry(data)
-
-
 # def listener():
 #     rospy.Subscriber(ODLC_TOPIC, Odlc, odlc_callback)
 #     rospy.Subscriber(MAPS_TOPIC, Map, maps_callback)