Xuan0319/DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update code logging and readconfig

Browse Source
This commit is contained in:
RangeOfGlitching
2023-02-09 02:07:11 +08:00
parent d2f433c80f
commit 434d048547
14 changed files with 245 additions and 385 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/src/local_mqtt_pub_data_to_ros.py
-70
View File
@@ -1,70 +0,0 @@
#!/usr/bin/env python3
#coding:utf-8
# license removed for brevity
import ssl
import rospy
from std_msgs.msg import String
import paho.mqtt.client as mqtt
# Ros
def ros_pub(dataJson):
# global publisher, rate
# data = json.loads(dataJson)
publisher.publish(dataJson) #將date字串發布到topic
rate.sleep()
# print(f"publish data {data}")
# MQTT
def initialise_clients(clientname):
# callback assignment
initialise_client = mqtt.Client(clientname, False)
initialise_client.topic_ack = []
return initialise_client
def on_connect(client, userdata, flags, rc):
print("Connected with result code " + str(rc))
# client.subscribe(mqtt_config["topic"])
client.subscribe("data/sensor")
def on_message(client, userdata, msg):
# print(f"got {msg.payload.decode('utf-8')}")
# print(f"msg.topic {msg.payload.decode('utf-8')}")
ros_pub(msg.payload.decode('utf-8'))
if __name__ == '__main__':
# Mqtt
mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
client = initialise_clients("123456")
client.on_connect = on_connect
client.on_message = on_message
client.connect(mqtt_config["host"], mqtt_config["port"], 60)
# Ros
Mqtt_Node = 'publisher_py'
rospy.init_node(Mqtt_Node)
# initialize Ros node
topicName = 'uav_message'
publisher = rospy.Publisher(topicName,String,queue_size=10)
rate = rospy.Rate(10)
client.loop_forever()
# mqtt connect code list
# 0: Connection successful
# 1: Connection refused – incorrect protocol version
# 2: Connection refused – invalid client identifier
# 3: Connection refused – server unavailable
# 4: Connection refused – bad username or password
# 5: Connection refused – not authorised
# 6-255: Currently unused.
class_model/src/local_mqtt_sub_data_from_ros.py
-136
View File
@@ -1,136 +0,0 @@
#!/usr/bin/env python3
#coding:utf-8
import sys
import orjson
import paho.mqtt.client as mqtt
from traceback import print_tb
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
import time
mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/sensor"}
data ={}
# Ros
def callBack_imu(IMU):
gyro_x=str(IMU.linear_acceleration.x)
gyro_y=str(IMU.linear_acceleration.y)
gyro_z=str(IMU.linear_acceleration.z)
accel_x=str(IMU.angular_velocity.x)
accel_y=str(IMU.angular_velocity.y)
accel_z=str(IMU.angular_velocity.z)
dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
data.update(dataImuUpdate)
# print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n')
print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
def callBack_gps(GPS):
lat=int(GPS.latitude*10000000) #change the scale to centimeters
lon=int(GPS.longitude*10000000)
alt=int(GPS.altitude*100)
dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
data.update(dataGpsUpdate)
# dataJsonFormate = orjson.dumps(data)
# mqtt_Pub(message=dataJsonFormate)
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
def callBack_compass_hdg(Compass):
heading = int(Compass.data*100)
dataGpsUpdate = {"heading": heading}
data.update(dataGpsUpdate)
dataJsonFormate = orjson.dumps(data)
mqtt_Pub(message=dataJsonFormate)
def callBack_state(state):
mode = state.mode
dataGpsUpdate = {"mode": mode}
data.update(dataGpsUpdate)
# get parameter
def GetParam(self,param_name):
param = self.get_param_srv(param_name)
if param.success:
if param.value.integer != 0:
value = param.value.integer
else:
value = param.value.real
else:
rospy.logwarn("Parameter "+param_name+" not read")
value = 0
return value
# MQTT
def initialise_clients(clientname):
# callback assignment
initialise_client = mqtt.Client(clientname, False)
initialise_client.topic_ack = []
return initialise_client
# publish a message
def mqtt_Pub(message, topics = mqtt_config["topic"], waitForAck=False):
mid = client.publish(topics, message, 0)[1]
# print(f"just published {message} to topic")
if waitForAck:
while mid not in client.topic_ack:
print("wait for ack")
time.sleep(0.25)
client.topic_ack.remove(mid)
def on_publish(self, userdata, mid):
client.topic_ack.append(mid)
def on_connect(self, userdata, flags, rc):
print("Connected with result code " + str(rc))
if __name__ == '__main__':
# Mqtt
client = initialise_clients("client1")
client.on_publish = on_publish
client.on_connect = on_connect
client.connect(mqtt_config["host"], mqtt_config["port"], 60)
client.loop_start()
# Ros
nodeName = 'save_data_py'
rospy.init_node(nodeName)
ros_namespace = ""
if not rospy.has_param("namespace"):
print("using default namespace")
else:
rospy.get_param("namespace", ros_namespace)
print("using namespace "+ros_namespace)
ros_namespace="/drone1"
# topicName = 'data_topic'
# subscriber = rospy.Subscriber(ros_namespace+'/mavros/imu/data_raw',Imu,callBack_imu) #從topic獲取string再呼叫callback
subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/global',NavSatFix,callBack_gps) #從topic獲取string再呼叫callback
subscriber = rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg',Float64,callBack_compass_hdg) #從topic獲取string再呼叫callback
# subscriber = rospy.Subscriber(ros_namespace+"/mavros/next_command",String,callBack_message)
#subscriber = rospy.Subscriber(ros_namespace+'/mavros/mavros_msgs/State',Header,callBack_state)
# ID = rospy.get_param(ros_namespace+'/leader/droneID')
# subscriber = rospy.Subscriber('/mavros/rangefinder/rangefinder',Range,callBack_rng) #從topic獲取string再呼叫callback
# ID = rospy.GetParam("droneID")
# print(ID)
rospy.spin()
class_model/src/mqttPubMain.py
+37 -53
View File
@@ -7,6 +7,7 @@ import time
import utils import utils
import proto.flight_information_pb2 as flight_information_pb2 import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2 import proto.flyformatioln_pb2 as flyformatioln_pb2
import logging
import rospy import rospy
@@ -18,29 +19,11 @@ from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu from sensor_msgs.msg import Imu
from sensor_msgs.msg import Range from sensor_msgs.msg import Range
from geometry_msgs.msg import Vector3 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): def init_dataFormat(cfg:utils.Read_PUB_Config):
ros_namespace="/drone1" ros_namespace="/drone1"
if cfg.msg_format == "Proto": if cfg.msg_format == "Proto":
utils.Proto_msg_from_ros.client = client utils.Proto_msg_from_ros.client = client
@@ -59,25 +42,43 @@ def init_dataFormat(cfg):
rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps) 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/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
else: 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): 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): def on_publish(self, userdata, mid):
print("pub ok") 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__': if __name__ == '__main__':
# Read Config # Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml") 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) 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(__name__)
logger.setLevel(logging.INFO)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt # Mqtt
client.on_connect = on_connect client.on_connect = on_connect
@@ -90,31 +91,14 @@ if __name__ == '__main__':
# Ros # Ros
rosClient = cfg.ROSClientNamePub rosClient = cfg.ROSClientNamePub
rospy.init_node(rosClient) rospy.init_node(rosClient)
# init data format
init_dataFormat(cfg) init_dataFormat(cfg)
try:
rospy.spin()
except KeyboardInterrupt as e:
client.disconnect()
logger.info("End of program")
sys.exit()
rospy.spin()
'''
#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)
'''
class_model/src/mqttSubMain.py
+42 -12
View File
@@ -9,8 +9,10 @@ import proto.flight_information_pb2 as flight_information_pb2
import proto.flyformatioln_pb2 as flyformatioln_pb2 import proto.flyformatioln_pb2 as flyformatioln_pb2
import rospy import rospy
from std_msgs.msg import String 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": if cfg.msg_format == "Proto":
utils.Proto_msg_to_ros.rate = rospy.Rate(10) 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_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
@@ -32,15 +34,18 @@ def init_dataFormat(cfg):
utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Json_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt utils.Json_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
else: 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): def on_connect(self, userdata, flags, rc):
print("Connected with result code " + str(rc)) logging.info("Connected with result code " + str(rc))
client.subscribe(cfg.Flight_Information_topicToMqtt) 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): def on_publish(self, userdata, mid):
pass pass
@@ -48,16 +53,35 @@ def on_publish(self, userdata, mid):
if __name__ == '__main__': if __name__ == '__main__':
# Read Config # Read Config
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_SUB.yml")
# Read Config cfg = utils.Read_SUB_Config(FilePath)
cfg = utils.MQTT_ROS_Config(FilePath) client = utils.MQTTClient(cfg.MQTTClientNameSub)
print(cfg)
# 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.DEBUG)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# Mqtt # Mqtt
client = utils.MQTTClient(cfg.MQTTClientNameSub) client = utils.MQTTClient(cfg.MQTTClientNameSub)
client.on_connect = on_connect client.on_connect = on_connect
client.on_message = utils.Json_msg_to_ros.on_message
client.on_publish = on_publish client.on_publish = on_publish
client.on_disconnect = on_disconnect
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
@@ -66,5 +90,11 @@ if __name__ == '__main__':
# initialize # initialize
init_dataFormat(cfg) init_dataFormat(cfg)
client.loop_start() try:
rospy.spin() client.loop_start()
rospy.spin()
except KeyboardInterrupt:
client.loop_stop()
client.disconnect()
logger.info("End of program")
sys.exit(0)
class_model/src/proto/flight_information.proto
+1 -1
View File
@@ -1,5 +1,5 @@
syntax = 'proto3'; syntax = 'proto3';
package flight_information_proto;
// GPS + compass // GPS + compass
message GPS { message GPS {
class_model/src/proto/flight_information_pb2.py
+5 -5
View File
@@ -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.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flight_information_pb2', globals()) _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flight_information_pb2', globals())
if _descriptor._USE_C_DESCRIPTORS == False: if _descriptor._USE_C_DESCRIPTORS == False:
DESCRIPTOR._options = None DESCRIPTOR._options = None
_GPS._serialized_start=28 _GPS._serialized_start=54
_GPS._serialized_end=111 _GPS._serialized_end=137
_FLIGHT_INFORMATION_MESSAGE._serialized_start=113 _FLIGHT_INFORMATION_MESSAGE._serialized_start=139
_FLIGHT_INFORMATION_MESSAGE._serialized_end=194 _FLIGHT_INFORMATION_MESSAGE._serialized_end=245
# @@protoc_insertion_point(module_scope) # @@protoc_insertion_point(module_scope)
class_model/src/proto/flyformatioln.proto
+1 -1
View File
@@ -1,5 +1,5 @@
syntax = "proto3"; syntax = "proto3";
package fly_formation_proto;
// leader only // leader only
enum FLY_FORMATION{ enum FLY_FORMATION{
class_model/src/proto/flyformatioln_pb2.py
+5 -5
View File
@@ -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.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flyformatioln_pb2', globals()) _builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'flyformatioln_pb2', globals())
if _descriptor._USE_C_DESCRIPTORS == False: if _descriptor._USE_C_DESCRIPTORS == False:
DESCRIPTOR._options = None DESCRIPTOR._options = None
_FLY_FORMATION._serialized_start=124 _FLY_FORMATION._serialized_start=165
_FLY_FORMATION._serialized_end=303 _FLY_FORMATION._serialized_end=344
_FLY_FORMATION_MESSAGE._serialized_start=23 _FLY_FORMATION_MESSAGE._serialized_start=44
_FLY_FORMATION_MESSAGE._serialized_end=121 _FLY_FORMATION_MESSAGE._serialized_end=162
# @@protoc_insertion_point(module_scope) # @@protoc_insertion_point(module_scope)
class_model/src/tower.py
-5
View File
@@ -11,11 +11,6 @@ def on_connect(self, userdata, flags, rc):
connect_flag = True connect_flag = True
# def on_message(self, userdata, msg):
# print(f"Receive UAV_Z550 {msg.payload.decode('utf-8')}")
# print(f"Receive UAV_H380 {msg.payload.decode('utf-8')}")
# print("command: ", end="")
def initialise_clients(clientName): def initialise_clients(clientName):
# callback assignment # callback assignment
class_model/src/utils/mqttConfig.yml → class_model/src/utils/mqttConfig_PUB.yml
+10 -9
View File
@@ -1,21 +1,22 @@
MQTT_ROS: MQTT:
msg_format: Json msg_format: Json
MQTTClientNamePub: Drone650Pub MQTTClientNamePub: Drone550Pub
MQTTClientNameSub: Drone650Sub host: 192.168.50.118
host: 192.168.50.27
port: 1883 port: 1883
keepalive: 60 keepalive: 60
willTopic: CheckDoneConnect willTopic: CheckDoneConnect
willTopicQOS: 1 willTopicQOS: 1
lwt: Drone650 Gone Offline lwt: Dron550 Gone Offline
willRetain: False willRetain: False
# Mqtt topic # Mqtt topic
Flight_Information_topicToMqtt: drone/leader/Flight_Information Flight_Information_topicToMqtt: Drone550/Flight_Information
Fly_Formation_topicToMqtt: drone/leader/Formation Fly_Formation_topicToMqtt: Drone550/Formation
# Change formate qos # Change formate qos
Fly_Formation_topicToMqtt_QOS: 2 Fly_Formation_topicToMqtt_QOS: 2
#ROS #ROS
ROSClientNamePub: Drone650Pub ROS:
ROSClientNameSub: Drone650Sub ROSClientNamePub: Drone550Pub
ROStopicName_Flight_Information: Flight_Information_reciver ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
class_model/src/utils/mqttConfig_SUB.yml
+21
View File
@@ -0,0 +1,21 @@
MQTT:
msg_format: Json
MQTTClientNameSub: Drone550Sub
host: 192.168.50.118
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
willTopicQOS: 1
lwt: Dron550 Gone Offline
willRetain: False
# Mqtt topic
Flight_Information_topicToMqtt: Drone550/Flight_Information
Fly_Formation_topicToMqtt: Drone550/Formation
# Change formate qos
Fly_Formation_topicToMqtt_QOS: 2
ROS:
ROSClientNameSub: Drone550Sub
ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: sub.log
class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py
+5 -5
View File
@@ -25,13 +25,13 @@ class Proto_msg_to_ros:
def ros_pub(cls, dataProto): def ros_pub(cls, dataProto):
if dataProto.topic == cls.Flight_Information_topicToMqtt: if dataProto.topic == cls.Flight_Information_topicToMqtt:
proto_msg = cls.flight_information_msg.FromString(dataProto.payload) proto_msg = cls.flight_information_msg.FromString(dataProto.payload)
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True, use_integers_for_enums=True)
print(protoTOJson_msg)
cls.publisher_Flight_Information.publish(protoTOJson_msg) cls.publisher_Flight_Information.publish(protoTOJson_msg)
cls.rate.sleep() cls.rate.sleep()
elif dataProto.topic == cls.Fly_Formation_topicToMqtt: elif dataProto.topic == cls.Fly_Formation_topicToMqtt:
proto_msg = cls.fly_formation_msg.FromString(dataProto.payload) proto_msg = cls.fly_formation_msg.FromString(dataProto.payload)
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True) # protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True, use_integers_for_enums=True)
cls.publisher_Fly_Formation.publish(protoTOJson_msg) cls.publisher_Fly_Formation.publish(protoTOJson_msg)
cls.rate.sleep() cls.rate.sleep()
else: else:
@@ -55,10 +55,10 @@ class Json_msg_to_ros:
@classmethod @classmethod
def ros_pub(cls, dataJson): def ros_pub(cls, dataJson):
if dataJson.topic == cls.Flight_Information_topicToMqtt: if dataJson.topic == cls.Flight_Information_topicToMqtt:
cls.publisher_Flight_Information.publish(dataJson.payload.decode('utf-8')) cls.publisher_Flight_Information.publish(dataJson.payload.decode("UTF-8"))
cls.rate.sleep() cls.rate.sleep()
elif dataJson.topic == cls.Fly_Formation_topicToMqtt: elif dataJson.topic == cls.Fly_Formation_topicToMqtt:
cls.publisher_Fly_Formation.publish(dataJson.payload.decode('utf-8')) cls.publisher_Fly_Formation.publish(dataJson.payload.decode("UTF-8"))
cls.rate.sleep() cls.rate.sleep()
else: else:
print("topic not found") print("topic not found")
class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py
+17 -24
View File
@@ -4,21 +4,19 @@ import time
class Proto_msg_from_ros: class Proto_msg_from_ros:
#Protobuf #Protobuf
flight_information_msg = None flight_information_msg = None
fly_formation_msg = None # delcare in function fly_formation_msg = None
#Mqtt #Mqtt
client = None client = None
Flight_Information_topicToMqtt = None Flight_Information_topicToMqtt = None
Fly_Formation_topicToMqtt = None Fly_Formation_topicToMqtt = None
Fly_Formation_topicToMqtt_QOS = None Fly_Formation_topicToMqtt_QOS = None
# TODO: gps decimal point fix
@classmethod @classmethod
def callBack_gps(cls, GPS): def callBack_gps(cls, GPS):
# flight_information_msg = flight_information_pb2.flight_information_message()
cls.flight_information_msg.gps.LAT = GPS.latitude cls.flight_information_msg.gps.LAT = GPS.latitude
cls.flight_information_msg.gps.LON = GPS.longitude cls.flight_information_msg.gps.LON = GPS.longitude
cls.flight_information_msg.gps.ALT = GPS.altitude cls.flight_information_msg.gps.ALT = GPS.altitude
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
# print(cls.flight_information_msg)
@classmethod @classmethod
def callBack_compass_hdg(cls, Compass): def callBack_compass_hdg(cls, Compass):
@@ -32,18 +30,15 @@ class Proto_msg_from_ros:
@classmethod @classmethod
def callBack_fly_formation(cls, Formation): def callBack_fly_formation(cls, Formation):
cls.fly_formation_msg.velocity = Formation.velocity cls.fly_formation_msg.velocity = Formation.velocity
# cls.fly_formation_msg.fly_formation = flyformatioln_pb2.FLY_FORMATION_v cls.fly_formation_msg.fly_formation = Formation.type
cls.fly_formation_msg.fly_formation = 2
flyFormationMsg = cls.fly_formation_msg.SerializeToString() flyFormationMsg = cls.fly_formation_msg.SerializeToString()
cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS) cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)
@classmethod @classmethod
# publish a message
def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False, qos:int=0)->None: def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False, qos:int=0)->None:
mid = cls.client.publish(topics, message, qos)[1] mid = cls.client.publish(topics, message, qos)[1]
# print(f"just published {message} to topic")
if waitForAck: if waitForAck:
while mid not in cls.client.topic_ack: while mid not in cls.client.topic_ack:
print("wait for ack") print("wait for ack")
@@ -54,13 +49,14 @@ class Proto_msg_from_ros:
class Json_msg_from_ros: class Json_msg_from_ros:
data = {} GPS_Data = {}
Formation_Data = {}
client = None client = None
#Mqtt #Mqtt
Flight_Information_topicToMqtt = None Flight_Information_topicToMqtt = None
Fly_Formation_topicToMqtt = None Fly_Formation_topicToMqtt = None
Fly_Formation_topicToMqtt_QOS = None Fly_Formation_topicToMqtt_QOS = None
# TODO: remove str
@classmethod @classmethod
def callBack_imu(cls, IMU): def callBack_imu(cls, IMU):
gyro_x=str(IMU.linear_acceleration.x) gyro_x=str(IMU.linear_acceleration.x)
@@ -73,8 +69,7 @@ class Json_msg_from_ros:
dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z} dataImuUpdate = {"gyro_x": gyro_x, "gyro_y": gyro_y,"gyro_z":gyro_z, "accel_x": accel_x, "accel_y": accel_y, "accel_z": accel_z}
cls.data.update(dataImuUpdate) cls.data.update(dataImuUpdate)
# print ('gyro_x:'+gyro_x+'\n'+'gyro_y:'+gyro_y+'\n'+'gyro_z:'+gyro_z +'\n')
print ('accel_x:'+accel_x+'\n'+'accel_y:'+accel_y+'\n'+'accel_z:'+accel_z +'\n')
@classmethod @classmethod
def callBack_gps(cls, GPS): def callBack_gps(cls, GPS):
@@ -82,30 +77,28 @@ class Json_msg_from_ros:
lon=int(GPS.longitude*10000000) lon=int(GPS.longitude*10000000)
alt=int(GPS.altitude*100) alt=int(GPS.altitude*100)
dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt} dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt}
cls.data.update(dataGpsUpdate) cls.GPS_Data.update(dataGpsUpdate)
# dataJsonFormate = orjson.dumps(data) dataJsonFormate = orjson.dumps(cls.GPS_Data)
# mqtt_Pub(message=dataJsonFormate) cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
@classmethod
def callBack_state(cls, state):
mode = state.mode
dataGpsUpdate = {"mode": mode}
cls.data.update(dataGpsUpdate)
# TODO: does decode need utf8
@classmethod @classmethod
def callBack_compass_hdg(cls, Compass): def callBack_compass_hdg(cls, Compass):
heading = int(Compass.data*100) heading = int(Compass.data*100)
dataGpsUpdate = {"heading": heading} dataGpsUpdate = {"heading": heading}
cls.data.update(dataGpsUpdate) cls.data.update(dataGpsUpdate)
dataJsonFormate = orjson.dumps(cls.data).decode("UTF-8") dataJsonFormate = orjson.dumps(cls.data)
cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt) cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
@classmethod
def callBack_fly_formation(cls, Formation):
Formation_data = {"velocity": Formation.velocity, "type": Formation.type}
flyFormationMsg = orjson.dumps(Formation_data)
cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)
@classmethod @classmethod
def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False, qos:int=0): def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False, qos:int=0):
mid = cls.client.publish(topics, message, qos)[1] mid = cls.client.publish(topics, message, qos)[1]
# print(f"just published {message} to topic")
if waitForAck: if waitForAck:
while mid not in cls.client.topic_ack: while mid not in cls.client.topic_ack:
print("wait for ack") print("wait for ack")
class_model/src/utils/readConfig.py
+93 -51
View File
@@ -1,65 +1,107 @@
import yaml import yaml
class Config:
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
def __init__(self, inFileName): def __init__(self, inFileName):
#read YAML config and get EV3 section self.sectionNames = ["MQTT","ROS", "LOG"]
infile=open(inFileName,'r') self.options = {}
ymlcfg=yaml.safe_load(infile) self.inFileName = inFileName
infile.close()
eccfg=ymlcfg.get(self.sectionName,None)
if eccfg is None: raise Exception('Missing {} section in cfg file'.format(self.sectionName))
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 #iterate over options
for opt in self.options: for opts, ecfg in zip(self.options, ecfgs):
if opt in eccfg: for opt in self.options[opts]:
optval=eccfg[opt] 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))
#verify parameter type #create attributes on the fly
if type(optval) != self.options[opt][0]: setattr(self,opt,optval)
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))
else: else:
setattr(self,opt,None) if self.options[opts][opt][1]:
raise Exception("Missing mandatory parameter {}".format(self.opt))
else:
setattr(self,opt,None)
#string representation for class data
def __str__(self): def __str__(self):
return str(yaml.dump(self.__dict__,default_flow_style=False)) 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 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__': if __name__ == "__main__":
cfg=MQTT_ROS_Config("mqttConfig.yml") cfg=SUB_Config("mqttConfig_PUB.yml")
print(cfg) cfg.setAttribute()
print(cfg.msg_format)