diff --git a/class_model/src/mqttPubMain.py b/class_model/src/mqttPubMain.py new file mode 100644 index 0000000..c2e2638 --- /dev/null +++ b/class_model/src/mqttPubMain.py @@ -0,0 +1,111 @@ +#!/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() diff --git a/class_model/src/mqttSubMain.py b/class_model/src/mqttSubMain.py new file mode 100644 index 0000000..2dfdec5 --- /dev/null +++ b/class_model/src/mqttSubMain.py @@ -0,0 +1,66 @@ +#!/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 + +def init_dataFormat(cfg): + if cfg.msg_format == "Proto": + # utils.Proto_msg_to_ros.rate = rospy.Rate(10) + # utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) + # utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) + + utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message() + utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message() + + utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt + utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt + elif cfg.msg_format == "Json": + utils.Proto_msg_to_ros.rate = rospy.Rate(10) + utils.Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10) + utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10) + + utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt + utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt + else: + print("msg_format not found") + +def on_message(client, userdata, msg): + utils.Proto_msg_to_ros.ros_pub(msg) + +def on_connect(self, userdata, flags, rc): + print("Connected with result code " + str(rc)) + client.subscribe(cfg.Flight_Information_topicToMqtt) + + +def on_publish(self, userdata, mid): + pass + + +if __name__ == '__main__': + # Read Config + cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml") + + + # Mqtt + init_dataFormat(cfg) + client = utils.MQTTClient(cfg.MQTTClientNameSub) + 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.loop_forever() + + # Ros + # rospy.init_node(cfg.ROSClientNameSub) + # initialize + # init_dataFormat(cfg) + + + # rospy.spin() \ No newline at end of file