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;
}
//////////////////////////////////////////////////////
/*
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 Range
from geometry_msgs.msg import Vector3
'''
# test proto
import random
@@ -35,11 +35,13 @@ class fake_hdg():
class state():
def __init__(self):
self.mode = "test"
'''
def init_dataFormat(cfg):
ros_namespace="/drone1"
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()
@@ -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.Fly_Formation_topicToMqtt = cfg.Fly_Formation_topicToMqtt
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":
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
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:
print("msg_format not found")
@@ -62,16 +68,16 @@ def on_connect(self, userdata, flags, rc):
print("Connected with result code " + str(rc))
def on_publish(self, userdata, mid):
pass
print("pub ok")
if __name__ == '__main__':
# 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)
print(cfg)
# Setting Config
init_dataFormat(cfg)
# Mqtt
client.on_connect = on_connect
@@ -81,12 +87,23 @@ if __name__ == '__main__':
client.will_set(cfg.willTopic, cfg.lwt, cfg.willTopicQOS, cfg.willRetain)
client.loop_start()
# Ros
rosClient = cfg.ROSClientNamePub
rospy.init_node(rosClient)
init_dataFormat(cfg)
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)
@@ -100,12 +117,4 @@ if __name__ == '__main__':
# 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()
'''
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):
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.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()
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.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
utils.Json_msg_to_ros.rate = rospy.Rate(10)
utils.Json_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
utils.Json_msg_to_ros.publisher_Fly_Formation = rospy.Publisher(cfg.ROStopicName_Fly_Formation,String,queue_size=10)
client.on_message = utils.Json_msg_to_ros.on_message
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:
print("msg_format not found")
def on_message(client, userdata, msg):
utils.Proto_msg_to_ros.ros_pub(msg)
# def on_message(client, userdata, msg):
# utils.Json_msg_to_ros.ros_pub(msg)
def on_connect(self, userdata, flags, rc):
print("Connected with result code " + str(rc))
@@ -45,22 +48,23 @@ def on_publish(self, userdata, mid):
if __name__ == '__main__':
# 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
init_dataFormat(cfg)
client = utils.MQTTClient(cfg.MQTTClientNameSub)
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.connect(host=cfg.host, port=cfg.port, keepalive=cfg.keepalive)
client.loop_forever()
# Ros
# rospy.init_node(cfg.ROSClientNameSub)
rospy.init_node(cfg.ROSClientNameSub)
# initialize
# init_dataFormat(cfg)
init_dataFormat(cfg)
# rospy.spin()
client.loop_start()
rospy.spin()
class_model/src/utils/mqttConfig.yml
+3 -3
View File
@@ -1,8 +1,8 @@
MQTT_ROS:
msg_format: Proto
msg_format: Json
MQTTClientNamePub: Drone650Pub
MQTTClientNameSub: Drone650Sub
host: 192.168.50.117
host: 192.168.50.27
port: 1883
keepalive: 60
willTopic: CheckDoneConnect
@@ -18,4 +18,4 @@ MQTT_ROS:
ROSClientNamePub: Drone650Pub
ROSClientNameSub: Drone650Sub
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)
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()
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.publisher_Fly_Formation.publish(protoTOJson_msg)
cls.rate.sleep()
else:
print("topic not found")
@staticmethod
def on_message(client, userdata, msg):
Proto_msg_to_ros.ros_pub(msg)
class Json_msg_to_ros:
@@ -51,10 +55,15 @@ class Json_msg_to_ros:
@classmethod
def ros_pub(cls, dataJson):
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()
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")
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.ALT = GPS.altitude
# print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
# print(flight_information_msg)
# print(cls.flight_information_msg)
@classmethod
def callBack_compass_hdg(cls, Compass):