diff --git a/class_model/src/uavlinkPubMain.py b/class_model/src/uavlinkPubMain.py old mode 100644 new mode 100755 index 0e39c84..ad9721c --- a/class_model/src/uavlinkPubMain.py +++ b/class_model/src/uavlinkPubMain.py @@ -1,11 +1,14 @@ +#!/usr/bin/env python3 +#coding:utf-8 import serial import time import random -import utils import sys import os import proto.flight_information_pb2 as flight_information_pb2 import logging +from utils.readConfig import Read_PUB_Config +from utils.proto_uavlink_sub_data_from_ros import Proto_msg_from_ros import rospy from std_msgs.msg import String @@ -18,19 +21,20 @@ from sensor_msgs.msg import Range from geometry_msgs.msg import Vector3 -def init_dataFormat(cfg:utils.Read_PUB_Config): +def init_dataFormat(cfg: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 + Proto_msg_from_ros.sel = sel + Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message() - 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) + rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, Proto_msg_from_ros.callBack_gps) + rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, 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) + FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_PUB.yml") + cfg = Read_PUB_Config(FilePath) # set log log_format = "%(asctime)s - %(levelname)s - %(message)s" 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 index ddd6133..bd38d76 100644 --- a/class_model/src/utils/proto_uavlink_sub_data_from_ros.py +++ b/class_model/src/utils/proto_uavlink_sub_data_from_ros.py @@ -23,5 +23,5 @@ class Proto_msg_from_ros: 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() + readTenByte = cls.sel.read_all() print(readTenByte) diff --git a/class_model/src/utils/uavlinkConfig_PUB.yml b/class_model/src/utils/uavlinkConfig_PUB.yml index f7a74cc..b12a931 100644 --- a/class_model/src/utils/uavlinkConfig_PUB.yml +++ b/class_model/src/utils/uavlinkConfig_PUB.yml @@ -2,7 +2,7 @@ UAVLINK: msg_format: Proto uav_id: \x01\x01 baudrate: 250000 - port: /dev/ttyUSB2 + port: /dev/ttyUSB0 MQTT: "None" #ROS ROS: