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

work fine with ros

Browse Source
This commit is contained in:
RangeOfGlitching
2022-12-14 03:01:26 +08:00
parent f60edf119d
commit 965d0d33c8
6 changed files with 67 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/src/main.cpp
+1 -1
View File
@@ -36,7 +36,7 @@ int main(int argc, char **argv) {
return 0; return 0;
} }
//////////////////////////////////////////////////////
/* /*
ThreadGPSClass gps_object; ThreadGPSClass gps_object;
class_model/src/mqttPubMain.py
Regular → Executable
+24 -15
View File
@@ -18,7 +18,7 @@ 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 # test proto
import random import random
@@ -35,11 +35,13 @@ class fake_hdg():
class state(): class state():
def __init__(self): def __init__(self):
self.mode = "test" self.mode = "test"
'''
def init_dataFormat(cfg): def init_dataFormat(cfg):
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
utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message() utils.Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
@@ -47,11 +49,15 @@ def init_dataFormat(cfg):
utils.Proto_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt 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 = cfg.Fly_Formation_topicToMqtt
utils.Proto_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS 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)
elif cfg.msg_format == "Json": elif cfg.msg_format == "Json":
utils.Json_msg_from_ros.client = client utils.Json_msg_from_ros.client = client
utils.Json_msg_from_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt 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 = cfg.Fly_Formation_topicToMqtt
utils.Json_msg_from_ros.Fly_Formation_topicToMqtt_QOS = cfg.Fly_Formation_topicToMqtt_QOS 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)
else: else:
print("msg_format not found") print("msg_format not found")
@@ -62,16 +68,16 @@ def on_connect(self, userdata, flags, rc):
print("Connected with result code " + str(rc)) print("Connected with result code " + str(rc))
def on_publish(self, userdata, mid): def on_publish(self, userdata, mid):
pass print("pub ok")
if __name__ == '__main__': if __name__ == '__main__':
# Read Config # Read Config
cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml")
cfg = utils.MQTT_ROS_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub) client = utils.MQTTClient(cfg.MQTTClientNamePub)
print(cfg)
# Setting Config # Setting Config
init_dataFormat(cfg)
# Mqtt # Mqtt
client.on_connect = on_connect client.on_connect = on_connect
@@ -81,12 +87,23 @@ if __name__ == '__main__':
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain) client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start() client.loop_start()
# Ros
rosClient = cfg.ROSClientNamePub
rospy.init_node(rosClient)
init_dataFormat(cfg)
rospy.spin()
'''
#test proto #test proto
gps = fakeGps() gps = fakeGps()
hdg = fake_hdg() hdg = fake_hdg()
state = state() state = state()
while True: while True:
#test proto #test proto
gps.latitude = random.uniform(0, 10) gps.latitude = random.uniform(0, 10)
gps.longitude = random.uniform(0, 10) gps.longitude = random.uniform(0, 10)
gps.altitude = random.uniform(0, 10) gps.altitude = random.uniform(0, 10)
@@ -100,12 +117,4 @@ if __name__ == '__main__':
# utils.Json_msg_from_ros.callBack_compass_hdg(hdg) # utils.Json_msg_from_ros.callBack_compass_hdg(hdg)
# utils.Json_msg_from_ros.callBack_state(state) # utils.Json_msg_from_ros.callBack_state(state)
# time.sleep(1) # 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()
class_model/src/mqttSubMain.py
Regular → Executable
+24 -20
View File
@@ -12,27 +12,30 @@ from std_msgs.msg import String
def init_dataFormat(cfg): def init_dataFormat(cfg):
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)
# utils.Proto_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,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.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.flyformatioln_msg = flyformatioln_pb2.fly_formation_message()
client.on_message = utils.Proto_msg_to_ros.on_message
utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt utils.Proto_msg_to_ros.Flight_Information_topicToMqtt = cfg.Flight_Information_topicToMqtt
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
elif cfg.msg_format == "Json": elif cfg.msg_format == "Json":
utils.Proto_msg_to_ros.rate = rospy.Rate(10) utils.Json_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.Json_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.Json_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 client.on_message = utils.Json_msg_to_ros.on_message
utils.Proto_msg_to_ros.Fly_Formation_topicToMqtt = cfg.Fly_Formation_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
else: else:
print("msg_format not found") print("msg_format not found")
def on_message(client, userdata, msg): # def on_message(client, userdata, msg):
utils.Proto_msg_to_ros.ros_pub(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)) print("Connected with result code " + str(rc))
@@ -45,22 +48,23 @@ def on_publish(self, userdata, mid):
if __name__ == '__main__': if __name__ == '__main__':
# Read Config # Read Config
cfg = utils.MQTT_ROS_Config("utils/mqttConfig.yml") FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig.yml")
# Read Config
cfg = utils.MQTT_ROS_Config(FilePath)
print(cfg)
# Mqtt # Mqtt
init_dataFormat(cfg)
client = utils.MQTTClient(cfg.MQTTClientNameSub) client = utils.MQTTClient(cfg.MQTTClientNameSub)
client.on_connect = on_connect client.on_connect = on_connect
client.on_message = on_message client.on_message = utils.Json_msg_to_ros.on_message
client.on_publish = on_publish client.on_publish = on_publish
client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive) client.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.loop_forever()
# Ros # Ros
# rospy.init_node(cfg.ROSClientNameSub) rospy.init_node(cfg.ROSClientNameSub)
# initialize # initialize
# init_dataFormat(cfg) init_dataFormat(cfg)
client.loop_start()
# rospy.spin() rospy.spin()
class_model/src/utils/mqttConfig.yml
+3 -3
View File
@@ -1,8 +1,8 @@
MQTT_ROS: MQTT_ROS:
msg_format: Proto msg_format: Json
MQTTClientNamePub: Drone650Pub MQTTClientNamePub: Drone650Pub
MQTTClientNameSub: Drone650Sub MQTTClientNameSub: Drone650Sub
host: 192.168.50.117 host: 192.168.50.27
port: 1883 port: 1883
keepalive: 60 keepalive: 60
willTopic: CheckDoneConnect willTopic: CheckDoneConnect
@@ -18,4 +18,4 @@ MQTT_ROS:
ROSClientNamePub: Drone650Pub ROSClientNamePub: Drone650Pub
ROSClientNameSub: Drone650Sub ROSClientNameSub: Drone650Sub
ROStopicName_Flight_Information: Flight_Information_reciver ROStopicName_Flight_Information: Flight_Information_reciver
ROStopicName_Fly_Formation: Fly_Formation_reciver ROStopicName_Fly_Formation: Fly_Formation_reciver
class_model/src/utils/protoJson_mqtt_pub_data_to_ros.py
+14 -5
View File
@@ -27,16 +27,20 @@ class Proto_msg_to_ros:
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)
print(protoTOJson_msg) 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)
cls.publisher_Formation.publish(protoTOJson_msg) cls.publisher_Fly_Formation.publish(protoTOJson_msg)
cls.rate.sleep() cls.rate.sleep()
else: else:
print("topic not found") print("topic not found")
@staticmethod
def on_message(client, userdata, msg):
Proto_msg_to_ros.ros_pub(msg)
class Json_msg_to_ros: class Json_msg_to_ros:
@@ -51,10 +55,15 @@ 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")
@staticmethod
def on_message(client, userdata, msg):
Json_msg_to_ros.ros_pub(msg)
class_model/src/utils/protoJson_mqtt_sub_data_from_ros.py
+1 -1
View File
@@ -18,7 +18,7 @@ class Proto_msg_from_ros:
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 ('lat:'+lat+'\n'+'lon:'+lon+'\n')
# print(flight_information_msg) # print(cls.flight_information_msg)
@classmethod @classmethod
def callBack_compass_hdg(cls, Compass): def callBack_compass_hdg(cls, Compass):