You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
112 lines
3.4 KiB
Python
112 lines
3.4 KiB
Python
3 years ago
|
#!/usr/bin/env python3
|
||
|
|
#coding:utf-8
|
||
|
|
import paho.mqtt.client as mqtt
|
||
|
|
import os
|
||
|
|
import sys
|
||
|
|
import time
|
||
|
|
import utils
|
||
|
|
import proto.flight_information_pb2 as flight_information_pb2
|
||
|
|
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||
|
|
|
||
|
|
|
||
|
|
import rospy
|
||
|
|
from std_msgs.msg import String
|
||
|
|
from std_msgs.msg import Float64
|
||
|
|
from std_msgs.msg import Header
|
||
|
|
from mavros_msgs.srv import ParamGet
|
||
|
|
from sensor_msgs.msg import NavSatFix
|
||
|
|
from sensor_msgs.msg import Imu
|
||
|
|
from sensor_msgs.msg import Range
|
||
|
|
from geometry_msgs.msg import Vector3
|
||
|
|
|
||
|
|
# test proto
|
||
|
|
import random
|
||
|
|
|
||
|
|
class fakeGps():
|
||
|
|
def __init__(self):
|
||
|
|
self.latitude = 0.0
|
||
|
|
self.longitude = 20
|
||
|
|
self.altitude = 30
|
||
|
|
|
||
|
|
class fake_hdg():
|
||
|
|
def __init__(self):
|
||
|
|
self.data = 40
|
||
|
|
# test json
|
||
|
|
class state():
|
||
|
|
def __init__(self):
|
||
|
|
self.mode = "test"
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
def init_dataFormat(cfg):
|
||
|
|
if cfg.msg_format == "Proto":
|
||
|
|
utils.Proto_msg_from_ros.client = client
|
||
|
|
utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||
|
|
utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
|
||
|
|
utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||
|
|
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||
|
|
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
|
||
|
|
elif cfg.msg_format == "Json":
|
||
|
|
utils.Json_msg_from_ros.client = client
|
||
|
|
utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||
|
|
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||
|
|
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS
|
||
|
|
else:
|
||
|
|
print("msg_format not found")
|
||
|
|
|
||
|
|
def on_message(client, userdata, msg):
|
||
|
|
pass
|
||
|
|
|
||
|
|
def on_connect(self, userdata, flags, rc):
|
||
|
|
print("Connected with result code " + str(rc))
|
||
|
|
|
||
|
|
def on_publish(self, userdata, mid):
|
||
|
|
pass
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
# Read Config
|
||
|
|
cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
|
||
|
|
client = utils.MQTTClient(cfg.MQTTClientNamePub)
|
||
|
|
|
||
|
|
# Setting Config
|
||
|
|
init_dataFormat(cfg)
|
||
|
|
|
||
|
|
# Mqtt
|
||
|
|
client.on_connect = on_connect
|
||
|
|
client.on_message = on_message
|
||
|
|
client.on_publish = on_publish
|
||
|
|
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
|
||
|
|
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
|
||
|
|
client.loop_start()
|
||
|
|
|
||
|
|
#test proto
|
||
|
|
gps = fakeGps()
|
||
|
|
hdg = fake_hdg()
|
||
|
|
state = state()
|
||
|
|
while True:
|
||
|
|
#test proto
|
||
|
|
gps.latitude = random.uniform(0, 10)
|
||
|
|
gps.longitude = random.uniform(0, 10)
|
||
|
|
gps.altitude = random.uniform(0, 10)
|
||
|
|
hdg.data = random.uniform(0, 10)
|
||
|
|
|
||
|
|
utils.Proto_msg_from_ros.callBack_gps(gps)
|
||
|
|
utils.Proto_msg_from_ros.callBack_compass_hdg(hdg)
|
||
|
|
time.sleep(0.25)
|
||
|
|
#test json
|
||
|
|
# utils.Json_msg_from_ros.callBack_gps(gps)
|
||
|
|
# utils.Json_msg_from_ros.callBack_compass_hdg(hdg)
|
||
|
|
# utils.Json_msg_from_ros.callBack_state(state)
|
||
|
|
# time.sleep(1)
|
||
|
|
|
||
|
|
# Ros
|
||
|
|
# rosClient = cfg.ROSClientNamePub
|
||
|
|
# rospy.init_node(rosClient)
|
||
|
|
# subscriber = rospy.Subscriber('/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
|
||
|
|
# subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
|
||
|
|
|
||
|
|
|
||
|
|
# rospy.spin()
|