|
|
|
|
#!/usr/bin/env python3
|
|
|
|
|
#coding:utf-8
|
|
|
|
|
import serial
|
|
|
|
|
import time
|
|
|
|
|
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
|
|
|
|
|
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:Read_PUB_Config):
|
|
|
|
|
f1data = b'\xf1drone550pub...........\r\n'
|
|
|
|
|
sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0.3)
|
|
|
|
|
sel.write(f1data)
|
|
|
|
|
readTenByte = sel.readline()
|
|
|
|
|
print(readTenByte)
|
|
|
|
|
Proto_msg_from_ros.sel = sel
|
|
|
|
|
Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
|
|
|
|
|
|
|
|
|
rospy.Subscriber('/mavros/global_position/global', NavSatFix, Proto_msg_from_ros.callBack_gps)
|
|
|
|
|
rospy.Subscriber('/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","uavlinkConfig_PUB.yml")
|
|
|
|
|
cfg = 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("__UAVLINKSUBPUB__")
|
|
|
|
|
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()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
