From d3059c3ac5fffae78f74af535bbfdfc9f9bf13cf Mon Sep 17 00:00:00 2001 From: RangeOfGlitching Date: Fri, 24 Mar 2023 23:23:54 +0800 Subject: [PATCH] uavlink with ros no check --- class_model/src/uavlinkPubMain.py | 71 +++++++++++++++++++ .../utils/proto_uavlink_sub_data_from_ros.py | 27 +++++++ class_model/src/utils/readConfig.py | 27 ++++--- class_model/src/utils/uavlinkConfig_PUB.yml | 12 ++++ 4 files changed, 126 insertions(+), 11 deletions(-) create mode 100644 class_model/src/uavlinkPubMain.py create mode 100644 class_model/src/utils/proto_uavlink_sub_data_from_ros.py create mode 100644 class_model/src/utils/uavlinkConfig_PUB.yml diff --git a/class_model/src/uavlinkPubMain.py b/class_model/src/uavlinkPubMain.py new file mode 100644 index 0000000..0e39c84 --- /dev/null +++ b/class_model/src/uavlinkPubMain.py @@ -0,0 +1,71 @@ +import serial +import time +import random +import utils +import sys +import os +import proto.flight_information_pb2 as flight_information_pb2 +import logging + +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 + + +def init_dataFormat(cfg:utils.Read_PUB_Config): + ros_namespace="/drone1" + f1data = b'\xf1drone550..............\r\n' + sel = serial.Serial(cfg.port, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE) + sel.write(f1data) + utils.Proto_msg_from_ros.sel = sel + + rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps) + rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg) + +if __name__ == '__main__': + FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml") + cfg = utils.Read_PUB_Config(FilePath) + + # set log + log_format = "%(asctime)s - %(levelname)s - %(message)s" + formatter = logging.Formatter(log_format) + + stream_handler = logging.StreamHandler() + stream_handler.setFormatter(formatter) + stream_handler.setLevel(logging.DEBUG) + + file_handler = logging.FileHandler(cfg.logFileName) + file_handler.setFormatter(formatter) + file_handler.setLevel(logging.INFO) + + logger = logging.getLogger(__name__) + logger.setLevel(logging.INFO) + logger.addHandler(file_handler) + logger.addHandler(stream_handler) + logger.info(cfg) + + + + # Ros + rosClient = cfg.ROSClientNamePub + rospy.init_node(rosClient) + + # init data format + init_dataFormat(cfg) + + try: + rospy.spin() + except BaseException as error: + logger.info("End of program") + sys.exit() + + + + + diff --git a/class_model/src/utils/proto_uavlink_sub_data_from_ros.py b/class_model/src/utils/proto_uavlink_sub_data_from_ros.py new file mode 100644 index 0000000..ddd6133 --- /dev/null +++ b/class_model/src/utils/proto_uavlink_sub_data_from_ros.py @@ -0,0 +1,27 @@ +import orjson +import time +import logging + +# logger = logging.getLogger("__PUB__") + +class Proto_msg_from_ros: + #Protobuf + flight_information_msg = None + sel = None + + + @classmethod + def callBack_gps(cls, GPS): + cls.flight_information_msg.gps.LAT = GPS.latitude + cls.flight_information_msg.gps.LON = GPS.longitude + cls.flight_information_msg.gps.ALT = GPS.altitude + # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') + # print(cls.flight_information_msg) + + @classmethod + def callBack_compass_hdg(cls, Compass): + cls.flight_information_msg.heading = Compass.data + flightInformationMsg = cls.flight_information_msg.SerializeToString() + cls.sel.write(b'\xf2' + flightInformationMsg + b'\r\n') + readTenByte = cls.sel.readline() + print(readTenByte) diff --git a/class_model/src/utils/readConfig.py b/class_model/src/utils/readConfig.py index 03bb330..a5ad7f6 100644 --- a/class_model/src/utils/readConfig.py +++ b/class_model/src/utils/readConfig.py @@ -2,7 +2,7 @@ import yaml class Config: def __init__(self, inFileName): - self.sectionNames = ["MQTT","ROS", "LOG"] + self.sectionNames = ["MQTT","ROS", "LOG", "UAVLINK"] self.options = {} self.inFileName = inFileName @@ -43,24 +43,29 @@ class Read_PUB_Config(Config): super().__init__(inFileName) self.options = { self.sectionNames[0]:{ - "msg_format": (str,True), - "MQTTClientNamePub": (str,True), - "host": (str,True), - "port": (int,True), - "keepalive": (int,True), - "willTopic":(str,True), - "lwt":(str, True), + "msg_format": (str,False), + "MQTTClientNamePub": (str,False), + "host": (str,False), + "port": (int,False), + "keepalive": (int,False), + "willTopic":(str,False), + "lwt":(str, False), "willRetain":(bool,False), - "willTopicQOS":(int,True), + "willTopicQOS":(int,False), "Flight_Information_topicToMqtt": (str,False), "Fly_Formation_topicToMqtt": (str,False), "Fly_Formation_topicToMqtt_QOS":(int,False)}, self.sectionNames[1]:{ - "ROSClientNamePub": (str,False), + "ROSClientNamePub": (str,True), "ROStopicName_Flight_Information": (str,False), "ROStopicName_Fly_Formation": (str,False)}, self.sectionNames[2]:{ - "logFileName":(str,False)}} + "logFileName":(str,True)}, + self.sectionNames[3]:{ + "msg_format": (str,False), + "uav_id": (str,False), + "baudrate": (int,False), + "port": (str,False)}} self.setAttribute() def __str__(self): diff --git a/class_model/src/utils/uavlinkConfig_PUB.yml b/class_model/src/utils/uavlinkConfig_PUB.yml new file mode 100644 index 0000000..f7a74cc --- /dev/null +++ b/class_model/src/utils/uavlinkConfig_PUB.yml @@ -0,0 +1,12 @@ +UAVLINK: + msg_format: Proto + uav_id: \x01\x01 + baudrate: 250000 + port: /dev/ttyUSB2 +MQTT: "None" +#ROS +ROS: + ROSClientNamePub: Drone550UAVLINKPub + ROStopicName_Flight_Information: Flight_Information_reciver +LOG: + logFileName: UAVLINKpub.log