local test without ros
MajorTop
3 years ago
parent
390d977c4d
commit
bb0db35248
@ -1,130 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import sys
|
||||
import json
|
||||
import paho.mqtt.client as mqtt
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import google.protobuf.json_format as json_format
|
||||
import datetime
|
||||
# import proto.duration_pb2 as duration_pb2
|
||||
|
||||
|
||||
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
|
||||
flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/pub"}
|
||||
|
||||
# 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):
|
||||
# flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
a = round(GPS.latitude, 3)
|
||||
flight_information_msg.gps.LAT = GPS.latitude
|
||||
flight_information_msg.gps.LON = GPS.longitude
|
||||
flight_information_msg.gps.ALT = GPS.altitude
|
||||
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
|
||||
# print(flight_information_msg)
|
||||
|
||||
|
||||
def callBack_compass_hdg(Compass):
|
||||
flight_information_msg.heading = round(Compass.data, 2)
|
||||
mqtt_Pub(message=flight_information_msg)
|
||||
# print(flight_information_msg)
|
||||
|
||||
|
||||
# def callBack_rng(RNG):
|
||||
# dataRngUpdate = {"range": RNG.range}
|
||||
# data.update(dataRngUpdate)
|
||||
# dataJsonFormate = json.dumps(data)
|
||||
# # print ('range:'+range+'\n')
|
||||
# print (dataJsonFormate)
|
||||
# mqtt_Pub(message=dataJsonFormate)
|
||||
|
||||
|
||||
# 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):
|
||||
# json = json_format.MessageToJson(message, indent=None, preserving_proto_field_name=True)
|
||||
message = message.SerializeToString()
|
||||
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)
|
||||
print("send")
|
||||
|
||||
|
||||
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('/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,50 +0,0 @@
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import google.protobuf.json_format as json_format
|
||||
import datetime
|
||||
import proto.duration_pb2 as duration_pb2
|
||||
import google.protobuf.internal.well_known_types as well_known_types
|
||||
|
||||
def flight_information():
|
||||
flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
flight_information_msg.gps.LAT = 34123.1231515
|
||||
flight_information_msg.gps.LON = 23423.1231515
|
||||
flight_information_msg.gps.ALT = 12123.1231515
|
||||
flight_information_msg.heading = 155.12215
|
||||
return flight_information_msg
|
||||
|
||||
|
||||
def fly_formation():
|
||||
fly_formation_msg = flyformatioln_pb2.fly_formation_message()
|
||||
fly_formation_msg.velocity = 128
|
||||
fly_formation_msg.fly_formation = flyformatioln_pb2.FLY_FORMATION_v
|
||||
return fly_formation_msg
|
||||
|
||||
|
||||
def file(message):
|
||||
path = "fly_formation.bin"
|
||||
print("write to file")
|
||||
with open(path, "wb") as f:
|
||||
f.write(message.SerializeToString())
|
||||
|
||||
with open(path, "rb") as f:
|
||||
msg = message.FromString(f.read())
|
||||
print(msg)
|
||||
|
||||
def dura():
|
||||
t = duration_pb2.Duration()
|
||||
t.seconds=1
|
||||
t.ToMilliseconds()
|
||||
print(t.ToMilliseconds())
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# flight_information_serial = flight_information()
|
||||
# fly_formation_serial = fly_formation()
|
||||
#
|
||||
#
|
||||
# file(fly_formation_serial)
|
||||
# json = to_json(complexfun())
|
||||
# print(json)
|
||||
# print(from_json(json, complexfun))
|
||||
dura()
|
||||
@ -0,0 +1,111 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import paho.mqtt.client as mqtt
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import utils
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
|
||||
|
||||
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
|
||||
|
||||
# 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):
|
||||
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()
|
||||
utils.Proto_msg_from_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
|
||||
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
|
||||
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
|
||||
else:
|
||||
print("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))
|
||||
|
||||
def on_publish(self, userdata, mid):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Read Config
|
||||
cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
|
||||
client = utils.MQTTClient(cfg.MQTTClientNamePub)
|
||||
|
||||
# Setting Config
|
||||
init_dataFormat(cfg)
|
||||
|
||||
# Mqtt
|
||||
client.on_connect = on_connect
|
||||
client.on_message = on_message
|
||||
client.on_publish = on_publish
|
||||
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()
|
||||
|
||||
#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)
|
||||
|
||||
# Ros
|
||||
# rosClient = cfg.ROSClientNamePub
|
||||
# rospy.init_node(rosClient)
|
||||
# subscriber = rospy.Subscriber('/mavros/global_position/global', NavSatFix, utils.Json_msg_from_ros.callBack_gps)
|
||||
# subscriber = rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, utils.Json_msg_from_ros.callBack_compass_hdg)
|
||||
|
||||
|
||||
# rospy.spin()
|
||||
@ -0,0 +1,66 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import paho.mqtt.client as mqtt
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import utils
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
|
||||
def init_dataFormat(cfg):
|
||||
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)
|
||||
# utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
|
||||
|
||||
utils.Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
|
||||
utils.Proto_msg_to_ros.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
|
||||
|
||||
utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
elif cfg.msg_format == "Json":
|
||||
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_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
|
||||
|
||||
utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
|
||||
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
|
||||
else:
|
||||
print("msg_format not found")
|
||||
|
||||
def on_message(client, userdata, msg):
|
||||
utils.Proto_msg_to_ros.ros_pub(msg)
|
||||
|
||||
def on_connect(self, userdata, flags, rc):
|
||||
print("Connected with result code " + str(rc))
|
||||
client.subscribe(cfg.Flight_Information_topicToMqtt)
|
||||
|
||||
|
||||
def on_publish(self, userdata, mid):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Read Config
|
||||
cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml")
|
||||
|
||||
|
||||
# Mqtt
|
||||
init_dataFormat(cfg)
|
||||
client = utils.MQTTClient(cfg.MQTTClientNameSub)
|
||||
client.on_connect = on_connect
|
||||
client.on_message = on_message
|
||||
client.on_publish = on_publish
|
||||
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
|
||||
client.loop_forever()
|
||||
|
||||
# Ros
|
||||
# rospy.init_node(cfg.ROSClientNameSub)
|
||||
# initialize
|
||||
# init_dataFormat(cfg)
|
||||
|
||||
|
||||
# rospy.spin()
|
||||
@ -0,0 +1,78 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# source: duration.proto
|
||||
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import message as _message
|
||||
from google.protobuf import reflection as _reflection
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_sym_db = _symbol_database.Default()
|
||||
|
||||
|
||||
|
||||
|
||||
DESCRIPTOR = _descriptor.FileDescriptor(
|
||||
name='duration.proto',
|
||||
package='google.protobuf',
|
||||
syntax='proto3',
|
||||
serialized_options=b'\n\023com.google.protobufB\rDurationProtoP\001Z1google.golang.org/protobuf/types/known/durationpb\370\001\001\242\002\003GPB\252\002\036Google.Protobuf.WellKnownTypes',
|
||||
create_key=_descriptor._internal_create_key,
|
||||
serialized_pb=b'\n\x0e\x64uration.proto\x12\x0fgoogle.protobuf\"*\n\x08\x44uration\x12\x0f\n\x07seconds\x18\x01 \x01(\x03\x12\r\n\x05nanos\x18\x02 \x01(\x05\x42\x83\x01\n\x13\x63om.google.protobufB\rDurationProtoP\x01Z1google.golang.org/protobuf/types/known/durationpb\xf8\x01\x01\xa2\x02\x03GPB\xaa\x02\x1eGoogle.Protobuf.WellKnownTypesb\x06proto3'
|
||||
)
|
||||
|
||||
|
||||
|
||||
|
||||
_DURATION = _descriptor.Descriptor(
|
||||
name='Duration',
|
||||
full_name='google.protobuf.Duration',
|
||||
filename=None,
|
||||
file=DESCRIPTOR,
|
||||
containing_type=None,
|
||||
create_key=_descriptor._internal_create_key,
|
||||
fields=[
|
||||
_descriptor.FieldDescriptor(
|
||||
name='seconds', full_name='google.protobuf.Duration.seconds', index=0,
|
||||
number=1, type=3, cpp_type=2, label=1,
|
||||
has_default_value=False, default_value=0,
|
||||
message_type=None, enum_type=None, containing_type=None,
|
||||
is_extension=False, extension_scope=None,
|
||||
serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
|
||||
_descriptor.FieldDescriptor(
|
||||
name='nanos', full_name='google.protobuf.Duration.nanos', index=1,
|
||||
number=2, type=5, cpp_type=1, label=1,
|
||||
has_default_value=False, default_value=0,
|
||||
message_type=None, enum_type=None, containing_type=None,
|
||||
is_extension=False, extension_scope=None,
|
||||
serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key),
|
||||
],
|
||||
extensions=[
|
||||
],
|
||||
nested_types=[],
|
||||
enum_types=[
|
||||
],
|
||||
serialized_options=None,
|
||||
is_extendable=False,
|
||||
syntax='proto3',
|
||||
extension_ranges=[],
|
||||
oneofs=[
|
||||
],
|
||||
serialized_start=35,
|
||||
serialized_end=77,
|
||||
)
|
||||
|
||||
DESCRIPTOR.message_types_by_name['Duration'] = _DURATION
|
||||
_sym_db.RegisterFileDescriptor(DESCRIPTOR)
|
||||
|
||||
Duration = _reflection.GeneratedProtocolMessageType('Duration', (_message.Message,), {
|
||||
'DESCRIPTOR' : _DURATION,
|
||||
'__module__' : 'duration_pb2'
|
||||
# @@protoc_insertion_point(class_scope:google.protobuf.Duration)
|
||||
})
|
||||
_sym_db.RegisterMessage(Duration)
|
||||
|
||||
|
||||
DESCRIPTOR._options = None
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@ -0,0 +1,14 @@
|
||||
syntax = 'proto3';
|
||||
|
||||
// GPS + compass
|
||||
|
||||
message GPS {
|
||||
optional float LAT = 1;
|
||||
optional float LON = 2;
|
||||
optional float ALT = 3;
|
||||
}
|
||||
|
||||
message flight_information_message {
|
||||
GPS gps = 1;
|
||||
optional float heading = 2;
|
||||
}
|
||||
@ -0,0 +1,27 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# source: flight_information.proto
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf.internal import builder as _builder
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_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')
|
||||
|
||||
_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
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@ -0,0 +1,17 @@
|
||||
syntax = "proto3";
|
||||
|
||||
// leader only
|
||||
|
||||
enum FLY_FORMATION{
|
||||
FLY_FORMATION_UNSPECIFIED = 0;
|
||||
FLY_FORMATION_v = 1;
|
||||
FLY_FORMATION_X = 2;
|
||||
FLY_FORMATION_O = 3;
|
||||
FLY_FORMATION_LINE = 4;
|
||||
FLY_FORMATION_ROW = 5;
|
||||
FLY_FORMATION_HEX = 6;
|
||||
}
|
||||
message fly_formation_message{
|
||||
optional float velocity = 1;
|
||||
FLY_FORMATION fly_formation= 2;
|
||||
}
|
||||
@ -0,0 +1,27 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
# Generated by the protocol buffer compiler. DO NOT EDIT!
|
||||
# source: flyformatioln.proto
|
||||
"""Generated protocol buffer code."""
|
||||
from google.protobuf.internal import builder as _builder
|
||||
from google.protobuf import descriptor as _descriptor
|
||||
from google.protobuf import descriptor_pool as _descriptor_pool
|
||||
from google.protobuf import symbol_database as _symbol_database
|
||||
# @@protoc_insertion_point(imports)
|
||||
|
||||
_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')
|
||||
|
||||
_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
|
||||
# @@protoc_insertion_point(module_scope)
|
||||
@ -1 +1 @@
|
||||
{"serialize":{"json":0.710234867001418,"proto":2.2478437370155007},"deserializ":{"json":0.41798062800080515,"proto":0.49253950000274926}}
|
||||
{"serialize":{"json":0.35475994200533023,"proto":1.0138140169947292},"deserializ":{"json":0.41626033899956383,"proto":0.4805599120008992}}
|
||||
@ -0,0 +1,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.basicMqtt import MQTTClient
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,17 @@
|
||||
import paho.mqtt.client as mqtt
|
||||
import time
|
||||
class MQTTClient(mqtt.Client):
|
||||
|
||||
def __init__(self,cname,**kwargs):
|
||||
super().__init__(cname,**kwargs)
|
||||
self.last_pub_time=time.time()
|
||||
self.topic_ack=[]
|
||||
self.run_flag=True
|
||||
self.subscribe_flag=False
|
||||
self.bad_connection_flag=False
|
||||
self.connected_flag=True
|
||||
self.disconnect_flag=False
|
||||
self.disconnect_time=0.0
|
||||
self.pub_msg_count=0
|
||||
self.devices=[]
|
||||
|
||||
@ -0,0 +1,21 @@
|
||||
MQTT_ROS:
|
||||
msg_format: Proto
|
||||
MQTTClientNamePub: Drone650Pub
|
||||
MQTTClientNameSub: Drone650Sub
|
||||
host: 192.168.50.117
|
||||
port: 1883
|
||||
keepalive: 60
|
||||
willTopic: CheckDoneConnect
|
||||
willTopicQOS: 1
|
||||
lwt: Drone650 Gone Offline
|
||||
willRetain: False
|
||||
# Mqtt topic
|
||||
Flight_Information_topicToMqtt: drone/leader/Flight_Information
|
||||
Fly_Formation_topicToMqtt: drone/leader/Formation
|
||||
# Change formate qos
|
||||
Fly_Formation_topicToMqtt_QOS: 2
|
||||
#ROS
|
||||
ROSClientNamePub: Drone650Pub
|
||||
ROSClientNameSub: Drone650Sub
|
||||
ROStopicName_Flight_Information: Flight_Information_reciver
|
||||
ROStopicName_Fly_Formation: Fly_Formation_reciver
|
||||
@ -0,0 +1,60 @@
|
||||
import orjson
|
||||
import time
|
||||
|
||||
import proto.flight_information_pb2 as flight_information_pb2
|
||||
import proto.flyformatioln_pb2 as flyformatioln_pb2
|
||||
import google.protobuf.json_format as json_format
|
||||
|
||||
class Proto_msg_to_ros:
|
||||
#Protobuf
|
||||
flight_information_msg = None
|
||||
fly_formation_msg = None # delcare in function
|
||||
|
||||
#Ros publisher
|
||||
rate = None
|
||||
publisher_Flight_Information = None
|
||||
publisher_Fly_Formation = None
|
||||
|
||||
#Mqtt topic: check data from which topic
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
|
||||
#Proto
|
||||
|
||||
@classmethod
|
||||
def ros_pub(cls, dataProto):
|
||||
if dataProto.topic == cls.Flight_Information_topicToMqtt:
|
||||
proto_msg = cls.flight_information_msg.FromString(dataProto.payload)
|
||||
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
|
||||
print(protoTOJson_msg)
|
||||
# cls.publisher_flight_information.publish(protoTOJson_msg)
|
||||
# cls.rate.sleep()
|
||||
elif dataProto.topic == cls.Fly_Formation_topicToMqtt:
|
||||
proto_msg = cls.fly_formation_msg.FromString(dataProto.payload)
|
||||
protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
|
||||
cls.publisher_Formation.publish(protoTOJson_msg)
|
||||
cls.rate.sleep()
|
||||
else:
|
||||
print("topic not found")
|
||||
|
||||
|
||||
|
||||
class Json_msg_to_ros:
|
||||
rate = None
|
||||
#Ros publisher
|
||||
publisher_Flight_Information = None
|
||||
publisher_Fly_Formation = None
|
||||
#Mqtt topic: check data from which topic
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
|
||||
@classmethod
|
||||
def ros_pub(cls, dataJson):
|
||||
if dataJson.topic == cls.Flight_Information_topicToMqtt:
|
||||
cls.publisher_flight_information.publish(dataJson.payload.decode('utf-8'))
|
||||
cls.rate.sleep()
|
||||
elif dataJson.topic == cls.Fly_Formation_topicToMqtt:
|
||||
cls.publisher_Fly_Formation.publish(dataJson.payload.decode('utf-8'))
|
||||
cls.rate.sleep()
|
||||
else:
|
||||
print("topic not found")
|
||||
Binary file not shown.
@ -0,0 +1,113 @@
|
||||
import orjson
|
||||
import time
|
||||
|
||||
class Proto_msg_from_ros:
|
||||
#Protobuf
|
||||
flight_information_msg = None
|
||||
fly_formation_msg = None # delcare in function
|
||||
#Mqtt
|
||||
client = None
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt_QOS = None
|
||||
|
||||
@classmethod
|
||||
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.LON = GPS.longitude
|
||||
cls.flight_information_msg.gps.ALT = GPS.altitude
|
||||
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
|
||||
# print(flight_information_msg)
|
||||
|
||||
@classmethod
|
||||
def callBack_compass_hdg(cls, Compass):
|
||||
cls.flight_information_msg.heading = Compass.data
|
||||
|
||||
flightInformationMsg = cls.flight_information_msg.SerializeToString()
|
||||
cls.mqtt_Pub(message=flightInformationMsg, topics=cls.Flight_Information_topicToMqtt)
|
||||
|
||||
|
||||
# TODO: Formation.velocity check
|
||||
@classmethod
|
||||
def callBack_fly_formation(cls, Formation):
|
||||
cls.fly_formation_msg.velocity = Formation.velocity
|
||||
# cls.fly_formation_msg.fly_formation = flyformatioln_pb2.FLY_FORMATION_v
|
||||
cls.fly_formation_msg.fly_formation = 2
|
||||
flyFormationMsg = cls.fly_formation_msg.SerializeToString()
|
||||
cls.mqtt_Pub(message=flyFormationMsg, topics=cls.Fly_Formation_topicToMqtt, qos=cls.Fly_Formation_topicToMqtt_QOS)
|
||||
|
||||
|
||||
|
||||
@classmethod
|
||||
# publish a message
|
||||
def mqtt_Pub(cls, message:bytes, topics:str, waitForAck:bool=False, qos:int=0)->None:
|
||||
mid = cls.client.publish(topics, message, qos)[1]
|
||||
# print(f"just published {message} to topic")
|
||||
if waitForAck:
|
||||
while mid not in cls.client.topic_ack:
|
||||
print("wait for ack")
|
||||
time.sleep(0.25)
|
||||
cls.client.topic_ack.remove(mid)
|
||||
|
||||
|
||||
|
||||
|
||||
class Json_msg_from_ros:
|
||||
data = {}
|
||||
client = None
|
||||
#Mqtt
|
||||
Flight_Information_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt = None
|
||||
Fly_Formation_topicToMqtt_QOS = None
|
||||
|
||||
@classmethod
|
||||
def callBack_imu(cls, 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}
|
||||
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
|
||||
def callBack_gps(cls, 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}
|
||||
cls.data.update(dataGpsUpdate)
|
||||
# dataJsonFormate = orjson.dumps(data)
|
||||
# mqtt_Pub(message=dataJsonFormate)
|
||||
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
|
||||
|
||||
@classmethod
|
||||
def callBack_state(cls, state):
|
||||
mode = state.mode
|
||||
dataGpsUpdate = {"mode": mode}
|
||||
cls.data.update(dataGpsUpdate)
|
||||
|
||||
@classmethod
|
||||
def callBack_compass_hdg(cls, Compass):
|
||||
heading = int(Compass.data*100)
|
||||
dataGpsUpdate = {"heading": heading}
|
||||
cls.data.update(dataGpsUpdate)
|
||||
dataJsonFormate = orjson.dumps(cls.data).decode("UTF-8")
|
||||
cls.mqtt_Pub(message=dataJsonFormate, topics=cls.Flight_Information_topicToMqtt)
|
||||
|
||||
|
||||
@classmethod
|
||||
def mqtt_Pub(cls, message:str, topics:str, waitForAck:bool=False, qos:int=0):
|
||||
mid = cls.client.publish(topics, message, qos)[1]
|
||||
# print(f"just published {message} to topic")
|
||||
if waitForAck:
|
||||
while mid not in cls.client.topic_ack:
|
||||
print("wait for ack")
|
||||
time.sleep(0.25)
|
||||
cls.client.topic_ack.remove(mid)
|
||||
@ -0,0 +1,65 @@
|
||||
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
|
||||
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))
|
||||
|
||||
#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))
|
||||
else:
|
||||
setattr(self,opt,None)
|
||||
|
||||
#string representation for class data
|
||||
def __str__(self):
|
||||
return str(yaml.dump(self.__dict__,default_flow_style=False))
|
||||
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
cfg=MQTT_ROS_Config("mqttConfig.yml")
|
||||
print(cfg)
|
||||
|
||||
|
||||
Loading…
Reference in New Issue