update code logging and readconfig
RangeOfGlitching
3 years ago
parent
d2f433c80f
commit
434d048547
@ -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()
|
|
||||||
@ -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
|
||||||
@ -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
|
||||||
@ -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))
|
|
||||||
|
|
||||||
#iterate over options
|
def setAttribute(self):
|
||||||
for opt in self.options:
|
with open(self.inFileName,"r") as f:
|
||||||
if opt in eccfg:
|
self.ymlcfg=yaml.safe_load(f)
|
||||||
optval=eccfg[opt]
|
|
||||||
|
|
||||||
|
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 opts, ecfg in zip(self.options, ecfgs):
|
||||||
|
for opt in self.options[opts]:
|
||||||
|
if opt in ecfg:
|
||||||
|
optval=ecfg[opt]
|
||||||
#verify parameter type
|
#verify parameter type
|
||||||
if type(optval) != self.options[opt][0]:
|
if type(optval) != self.options[opts][opt][0]:
|
||||||
raise Exception('Parameter "{}" has wrong type'.format(opt))
|
raise Exception("Parameter {} has wrong type".format(self.opt))
|
||||||
|
|
||||||
#create attributes on the fly
|
#create attributes on the fly
|
||||||
setattr(self,opt,optval)
|
setattr(self,opt,optval)
|
||||||
else:
|
else:
|
||||||
if self.options[opt][1]:
|
if self.options[opts][opt][1]:
|
||||||
raise Exception('Missing mandatory parameter "{}"'.format(opt))
|
raise Exception("Missing mandatory parameter {}".format(self.opt))
|
||||||
else:
|
else:
|
||||||
setattr(self,opt,None)
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue