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
|
||||
MQTTClientNamePub: Drone650Pub
|
||||
MQTTClientNameSub: Drone650Sub
|
||||
host: 192.168.50.27
|
||||
MQTTClientNamePub: Drone550Pub
|
||||
host: 192.168.50.118
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Drone650 Gone Offline
|
||||
lwt: Dron550 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Flight_Information_topicToMqtt: drone/leader/Flight_Information
|
||||
Fly_Formation_topicToMqtt: drone/leader/Formation
|
||||
Flight_Information_topicToMqtt: Drone550/Flight_Information
|
||||
Fly_Formation_topicToMqtt: Drone550/Formation
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
#ROS
|
||||
ROSClientNamePub: Drone650Pub
|
||||
ROSClientNameSub: Drone650Sub
|
||||
ROS:
|
||||
ROSClientNamePub: Drone550Pub
|
||||
ROStopicName_Flight_Information: Flight_Information_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
|
||||
|
||||
|
||||
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
|
||||
|
||||
#iterate over options
|
||||
for opt in self.options:
|
||||
if opt in eccfg:
|
||||
optval=eccfg[opt]
|
||||
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 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[opt][0]:
|
||||
raise Exception('Parameter "{}" has wrong type'.format(opt))
|
||||
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:
|
||||
if self.options[opt][1]:
|
||||
raise Exception('Missing mandatory parameter "{}"'.format(opt))
|
||||
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):
|
||||
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__':
|
||||
cfg=MQTT_ROS_Config("mqttConfig.yml")
|
||||
print(cfg)
|
||||
if __name__ == "__main__":
|
||||
cfg=SUB_Config("mqttConfig_PUB.yml")
|
||||
cfg.setAttribute()
|
||||
print(cfg.msg_format)
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue