log and rewrite readconfig

main
RangeOfGlitching 3 years ago
parent f6c2f80841
commit 831199bf93

@ -7,6 +7,7 @@ import time
import utils
import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import logging
import rospy
@ -28,9 +29,9 @@ class fakeGps():
self.longitude = 20
self.altitude = 30
# class fake_hdg():
# def __init__(self):
# self.data = 40
class fake_hdg():
def __init__(self):
self.data = 40
# class fake_formation():
# def __init__(self):
@ -43,8 +44,8 @@ class fakeGps():
def init_dataFormat(cfg):
ros_namespace="/drone1"
def init_dataFormat(cfg:utils.Read_PUB_Config):
# ros_namespace="/drone1"
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()
@ -52,41 +53,62 @@ def init_dataFormat(cfg):
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
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, 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)
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
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
# rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
# rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
else:
print("msg_format not found")
logging.debug("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))
logger.info("Connected with result code " + str(rc))
def on_publish(self, userdata, mid):
# print("pub ok")
pass
logger.debug("pub success")
def on_disconnect(client, userdata, rc):
logger.debug("disconnecting reason " +str(rc))
client.connected_flag=False
client.disconnect_flag=True
if __name__ == '__main__':
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml")
cfg = utils.MQTT_ROS_Config(FilePath)
cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub)
print(cfg)
# Setting Config
# 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("__PUB__")
logger.setLevel(logging.INFO)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt
client.on_connect = on_connect
client.on_message = on_message
client.on_publish = on_publish
client.on_disconnect = on_disconnect
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()
@ -102,25 +124,31 @@ if __name__ == '__main__':
#test proto
gps = fakeGps()
# hdg = fake_hdg()
hdg = fake_hdg()
# formation = fake_formation()
while True:
#test proto
# 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)
hdg.data = random.uniform(0, 10)
# formation.velocity = random.uniform(0, 10)
# 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_fly_formation(formation)
time.sleep(0.25)
'''
utils.Proto_msg_from_ros.callBack_gps(gps)
utils.Proto_msg_from_ros.callBack_compass_hdg(hdg)
utils.Proto_msg_from_ros.callBack_fly_formation(formation)
time.sleep(0.25)
'''
try:
# 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_fly_formation(formation)
# test proto
# utils.Proto_msg_from_ros.callBack_gps(gps)
# utils.Proto_msg_from_ros.callBack_compass_hdg(hdg)
## utils.Proto_msg_from_ros.callBack_fly_formation(formation)
time.sleep(0.25)
except KeyboardInterrupt as e:
client.disconnect()
logger.info("End of program")
sys.exit()

@ -9,8 +9,9 @@ import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import rospy
from std_msgs.msg import String
import logging
def init_dataFormat(cfg):
def init_dataFormat(cfg:utils.Read_SUB_Config):
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)
@ -32,17 +33,21 @@ def init_dataFormat(cfg):
utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
else:
print("msg_format not found")
logging.debug("msg_format not found")
# def on_message(client, userdata, msg):
# utils.Json_msg_to_ros.ros_pub(msg)
def on_connect(self, userdata, flags, rc):
print("Connected with result code " + str(rc))
print(cfg.Flight_Information_topicToMqtt)
logging.info("Connected with result code " + str(rc))
client.subscribe(cfg.Flight_Information_topicToMqtt)
client.subscribe(cfg.Fly_Formation_topicToMqtt)
def on_disconnect(client, userdata, rc):
# logging.info("disconnecting reason " +str(rc))
client.connected_flag=False
client.disconnect_flag=True
def on_publish(self, userdata, mid):
pass
@ -50,14 +55,34 @@ def on_publish(self, userdata, mid):
if __name__ == '__main__':
# Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB.yml")
# Read Config
cfg = utils.MQTT_ROS_Config(FilePath)
print(cfg)
cfg = utils.Read_SUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNameSub)
# 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("__SUB__")
logger.setLevel(logging.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt
client = utils.MQTTClient(cfg.MQTTClientNameSub)
client.on_connect = on_connect
client.on_publish = on_publish
client.on_disconnect = on_disconnect
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
@ -65,6 +90,13 @@ if __name__ == '__main__':
# rospy.init_node(cfg.ROSClientNameSub)
# initialize
init_dataFormat(cfg)
try:
client.loop_forever()
# rospy.spin()
except KeyboardInterrupt as e:
client.loop_stop()
client.disconnect()
logger.info("End of program")
sys.exit()
client.loop_forever()
# rospy.spin()

@ -1,5 +1,5 @@
syntax = 'proto3';
package flight_information_proto;
// GPS + compass
message GPS {

@ -13,15 +13,15 @@ _sym_db = _symbol_database.Default()
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x18\x66light_information.proto\"S\n\x03GPS\x12\x10\n\x03LAT\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12\x10\n\x03LON\x18\x02 \x01(\x02H\x01\x88\x01\x01\x12\x10\n\x03\x41LT\x18\x03 \x01(\x02H\x02\x88\x01\x01\x42\x06\n\x04_LATB\x06\n\x04_LONB\x06\n\x04_ALT\"Q\n\x1a\x66light_information_message\x12\x11\n\x03gps\x18\x01 \x01(\x0b\x32\x04.GPS\x12\x14\n\x07heading\x18\x02 \x01(\x02H\x00\x88\x01\x01\x42\n\n\x08_headingb\x06proto3')
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x18\x66light_information.proto\x12\x18\x66light_information_proto\"S\n\x03GPS\x12\x10\n\x03LAT\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12\x10\n\x03LON\x18\x02 \x01(\x02H\x01\x88\x01\x01\x12\x10\n\x03\x41LT\x18\x03 \x01(\x02H\x02\x88\x01\x01\x42\x06\n\x04_LATB\x06\n\x04_LONB\x06\n\x04_ALT\"j\n\x1a\x66light_information_message\x12*\n\x03gps\x18\x01 \x01(\x0b\x32\x1d.flight_information_proto.GPS\x12\x14\n\x07heading\x18\x02 \x01(\x02H\x00\x88\x01\x01\x42\n\n\x08_headingb\x06proto3')
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flight_information_pb2', globals())
if _descriptor._USE_C_DESCRIPTORS == False:
DESCRIPTOR._options = None
_GPS._serialized_start=28
_GPS._serialized_end=111
_FLIGHT_INFORMATION_MESSAGE._serialized_start=113
_FLIGHT_INFORMATION_MESSAGE._serialized_end=194
_GPS._serialized_start=54
_GPS._serialized_end=137
_FLIGHT_INFORMATION_MESSAGE._serialized_start=139
_FLIGHT_INFORMATION_MESSAGE._serialized_end=245
# @@protoc_insertion_point(module_scope)

@ -1,5 +1,5 @@
syntax = "proto3";
package fly_formation_proto;
// leader only
enum FLY_FORMATION{

@ -13,15 +13,15 @@ _sym_db = _symbol_database.Default()
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x13\x66lyformatioln.proto\"b\n\x15\x66ly_formation_message\x12\x15\n\x08velocity\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12%\n\rfly_formation\x18\x02 \x01(\x0e\x32\x0e.FLY_FORMATIONB\x0b\n\t_velocity*\xb3\x01\n\rFLY_FORMATION\x12\x1d\n\x19\x46LY_FORMATION_UNSPECIFIED\x10\x00\x12\x13\n\x0f\x46LY_FORMATION_v\x10\x01\x12\x13\n\x0f\x46LY_FORMATION_X\x10\x02\x12\x13\n\x0f\x46LY_FORMATION_O\x10\x03\x12\x16\n\x12\x46LY_FORMATION_LINE\x10\x04\x12\x15\n\x11\x46LY_FORMATION_ROW\x10\x05\x12\x15\n\x11\x46LY_FORMATION_HEX\x10\x06\x62\x06proto3')
DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x13\x66lyformatioln.proto\x12\x13\x66ly_formation_proto\"v\n\x15\x66ly_formation_message\x12\x15\n\x08velocity\x18\x01 \x01(\x02H\x00\x88\x01\x01\x12\x39\n\rfly_formation\x18\x02 \x01(\x0e\x32\".fly_formation_proto.FLY_FORMATIONB\x0b\n\t_velocity*\xb3\x01\n\rFLY_FORMATION\x12\x1d\n\x19\x46LY_FORMATION_UNSPECIFIED\x10\x00\x12\x13\n\x0f\x46LY_FORMATION_v\x10\x01\x12\x13\n\x0f\x46LY_FORMATION_X\x10\x02\x12\x13\n\x0f\x46LY_FORMATION_O\x10\x03\x12\x16\n\x12\x46LY_FORMATION_LINE\x10\x04\x12\x15\n\x11\x46LY_FORMATION_ROW\x10\x05\x12\x15\n\x11\x46LY_FORMATION_HEX\x10\x06\x62\x06proto3')
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flyformatioln_pb2', globals())
if _descriptor._USE_C_DESCRIPTORS == False:
DESCRIPTOR._options = None
_FLY_FORMATION._serialized_start=124
_FLY_FORMATION._serialized_end=303
_FLY_FORMATION_MESSAGE._serialized_start=23
_FLY_FORMATION_MESSAGE._serialized_end=121
_FLY_FORMATION._serialized_start=165
_FLY_FORMATION._serialized_end=344
_FLY_FORMATION_MESSAGE._serialized_start=44
_FLY_FORMATION_MESSAGE._serialized_end=162
# @@protoc_insertion_point(module_scope)

@ -2,5 +2,6 @@ from utils.protoJson_mqtt_pub_data_to_ros import Proto_msg_to_ros
from utils.protoJson_mqtt_pub_data_to_ros import Json_msg_to_ros
from utils.protoJson_mqtt_sub_data_from_ros import Proto_msg_from_ros
from utils.protoJson_mqtt_sub_data_from_ros import Json_msg_from_ros
from utils.readConfig import MQTT_ROS_Config
from utils.readConfig import Read_PUB_Config
from utils.readConfig import Read_SUB_Config
from utils.basicMqtt import MQTTClient

@ -1,8 +1,7 @@
MQTT_ROS:
MQTT:
msg_format: Json
MQTTClientNamePub: Drone550Pub
MQTTClientNameSub: Drone550Sub
host: 192.168.50.117
host: 192.168.50.118
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
@ -15,7 +14,9 @@ MQTT_ROS:
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
#ROS
ROS:
ROSClientNamePub: Drone550Pub
ROSClientNameSub: Drone550Sub
ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log

@ -1,8 +1,7 @@
MQTT_ROS:
MQTT:
msg_format: Json
MQTTClientNamePub: Drone550Pub
MQTTClientNameSub: Drone550Sub
host: 192.168.50.117
host: 192.168.50.118
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
@ -14,8 +13,9 @@ MQTT_ROS:
Fly_Formation_topicToMqtt: Drone550/Formation
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
#ROS
ROSClientNamePub: Drone550Pub
ROS:
ROSClientNameSub: Drone550Sub
ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: sub.log

@ -5,6 +5,7 @@ import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2
import google.protobuf.json_format as json_format
# TODO: use native ros type instead of json or str
class Proto_msg_to_ros:
#Protobuf
@ -58,12 +59,12 @@ class Json_msg_to_ros:
@classmethod
def ros_pub(cls, dataJson):
if dataJson.topic == cls.Flight_Information_topicToMqtt:
print(dataJson.payload.decode('utf-8'))
# cls.publisher_Flight_Information.publish(dataJson.payload.decode('utf-8'))
print(dataJson.payload.decode("UTF-8"))
# cls.publisher_Flight_Information.publish(dataJson.payload.decode("UTF-8"))
# cls.rate.sleep()
elif dataJson.topic == cls.Fly_Formation_topicToMqtt:
print(dataJson.payload.decode('utf-8'))
# cls.publisher_Fly_Formation.publish(dataJson.payload.decode('utf-8'))
print(dataJson.payload.decode("UTF-8"))
# cls.publisher_Fly_Formation.publish(dataJson.payload.decode("UTF-8"))
# cls.rate.sleep()
else:
print("topic not found")

@ -1,65 +1,107 @@
import yaml
class MQTT_ROS_Config:
"""
MQTT_ROS_Config configuration class
"""
# class variables
sectionName='MQTT_ROS'
options={
'msg_format': (str,True),
'MQTTClientNamePub': (str,True),
'MQTTClientNameSub': (str,True),
'host': (str,True),
'port': (int,True),
'keepalive': (int,True),
'willTopic':(str,True),
'lwt':(str, True),
'willRetain':(bool,False),
'willTopicQOS':(int,True),
'Flight_Information_topicToMqtt': (str,False),
'Fly_Formation_topicToMqtt': (str,False),
'Fly_Formation_topicToMqtt_QOS':(int,False),
'ROSClientNamePub': (str,False),
'ROSClientNameSub': (str,False),
'ROStopicName_Flight_Information': (str,False),
'ROStopicName_Fly_Formation': (str,False)}
#constructor
class Config:
def __init__(self, inFileName):
#read YAML config and get EV3 section
infile=open(inFileName,'r')
ymlcfg=yaml.safe_load(infile)
infile.close()
eccfg=ymlcfg.get(self.sectionName,None)
if eccfg is None: raise Exception('Missing {} section in cfg file'.format(self.sectionName))
self.sectionNames = ["MQTT","ROS", "LOG"]
self.options = {}
self.inFileName = inFileName
def setAttribute(self):
with open(self.inFileName,"r") as f:
self.ymlcfg=yaml.safe_load(f)
ecfgs = [self.ymlcfg.get(name) for name in self.sectionNames]
if None in ecfgs:
nameIndex = ecfgs.index(None)
raise Exception("Missing {} section in cfg file".format(self.sectionNames[nameIndex]))
#iterate over options
for opt in self.options:
if opt in eccfg:
optval=eccfg[opt]
#verify parameter type
if type(optval) != self.options[opt][0]:
raise Exception('Parameter "{}" has wrong type'.format(opt))
#create attributes on the fly
setattr(self,opt,optval)
else:
if self.options[opt][1]:
raise Exception('Missing mandatory parameter "{}"'.format(opt))
for opts, ecfg in zip(self.options, ecfgs):
for opt in self.options[opts]:
if opt in ecfg:
optval=ecfg[opt]
#verify parameter type
if type(optval) != self.options[opts][opt][0]:
raise Exception("Parameter {} has wrong type".format(self.opt))
#create attributes on the fly
setattr(self,opt,optval)
else:
setattr(self,opt,None)
#string representation for class data
if self.options[opts][opt][1]:
raise Exception("Missing mandatory parameter {}".format(self.opt))
else:
setattr(self,opt,None)
def __str__(self):
return str(yaml.dump(self.ymlcfg, default_flow_style=False))
class Read_PUB_Config(Config):
def setAttribute(self):
super().setAttribute()
def __init__(self, inFileName):
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),
"willRetain":(bool,False),
"willTopicQOS":(int,True),
"Flight_Information_topicToMqtt": (str,False),
"Fly_Formation_topicToMqtt": (str,False),
"Fly_Formation_topicToMqtt_QOS":(int,False)},
self.sectionNames[1]:{
"ROSClientNamePub": (str,False),
"ROStopicName_Flight_Information": (str,False),
"ROStopicName_Fly_Formation": (str,False)},
self.sectionNames[2]:{
"logFileName":(str,False)}}
self.setAttribute()
def __str__(self):
return str(yaml.dump(self.__dict__,default_flow_style=False))
return super().__str__()
class Read_SUB_Config(Config):
def setAttribute(self):
super().setAttribute()
def __init__(self, inFileName):
super().__init__(inFileName)
self.options = {
self.sectionNames[0]:{
"msg_format": (str,True),
"MQTTClientNameSub": (str,True),
"host": (str,True),
"port": (int,True),
"keepalive": (int,True),
"willTopic":(str,True),
"lwt":(str, True),
"willRetain":(bool,False),
"willTopicQOS":(int,True),
"Flight_Information_topicToMqtt": (str,False),
"Fly_Formation_topicToMqtt": (str,False),
"Fly_Formation_topicToMqtt_QOS":(int,False)},
self.sectionNames[1]:{
"ROSClientNameSub": (str,False),
"ROStopicName_Flight_Information": (str,False),
"ROStopicName_Fly_Formation": (str,False)},
self.sectionNames[2]:{
"logFileName":(str,False)}}
self.setAttribute()
def __str__(self):
return super().__str__()
if __name__ == '__main__':
cfg=MQTT_ROS_Config("mqttConfig.yml")
print(cfg)
if __name__ == "__main__":
cfg=SUB_Config("mqttConfig_PUB.yml")
cfg.setAttribute()
print(cfg.msg_format)

Loading…
Cancel
Save