Update local_mqtt_sub_data_from_ros.py

Stream/localRosData/local_mqtt_sub_data_from_ros.py

@@ -1,8 +1,7 @@
 #!/usr/bin/env python3
 #coding:utf-8
 import sys
-# import json
-import ujson
+import json
 import paho.mqtt.client as mqtt
 
 
@@ -29,32 +28,27 @@ def callBack_imu(IMU):
 
     dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
     data.update(dataImuUpdate)
-
     # print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n')
     # print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
 
 
 def callBack_gps(GPS):
-    lat=str(GPS.latitude)
-    lon=str(GPS.longitude)
+
+    lat=str(int(GPS.latitude*100000))
+    lon=str(int(GPS.longitude*100000))
     dataGpsUpdate = {"lat": GPS.latitude, "lon": GPS.longitude}
     data.update(dataGpsUpdate)
-    toJson(data)
     # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
+
 def callBack_rng(RNG):
     dataRngUpdate = {"range": RNG.range}
     data.update(dataRngUpdate)
-#     dataJsonFormate = json.dumps(data)
-    dataJsonFormate = ujson.dumps(data)
+    dataJsonFormate = json.dumps(data)
     # print ('range:'+range+'\n')
-    
-
-def toJson(data):
-#     dataJsonFormate = json.dumps(data)
-    dataJsonFormate = ujson.dumps(data)
     print (dataJsonFormate)
     mqtt_Pub(message=dataJsonFormate)
 
+
 # MQTT
 def initialise_clients(clientname):
     # callback assignment
@@ -93,7 +87,8 @@ if __name__ == '__main__':
     nodeName = 'save_data_py'
     rospy.init_node(nodeName)
     # topicName = 'data_topic'
-    subscriber1 = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu)  #從topic獲取string再呼叫callback
-    subscriber2 = rospy.Subscriber('/mavros/global_position/global',NavSatFix,callBack_gps)  #從topic獲取string再呼叫callback
-    subscriber3 = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng)  #從topic獲取string再呼叫callback
+    subscriber = rospy.Subscriber('/mavros/imu/data_raw',Imu,callBack_imu)  #從topic獲取string再呼叫callback
+    subscriber = rospy.Subscriber('/mavros/global_position/global',NavSatFix,callBack_gps)  #從topic獲取string再呼叫callback
+    subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng)  #從topic獲取string再呼叫callback
+
     rospy.spin()