diff --git a/class_model/src/MQTT_Pub_message.py b/class_model/src/MQTT_Pub_message.py deleted file mode 100755 index 6fea668..0000000 --- a/class_model/src/MQTT_Pub_message.py +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env python3 -#coding:utf-8 -# license removed for brevity -import ssl -import rospy -from std_msgs.msg import String -import paho.mqtt.client as mqtt -import json - - -# Ros -def ros_pub(dataJson): - global publisher, rate - # data = json.loads(dataJson) - publisher.publish(dataJson) #將date字串發布到topic - rate.sleep() - # print(f"publish data {data}") - -# MQTT -def initialise_clients(clientname): - # callback assignment - initialise_client = mqtt.Client(clientname, False) - initialise_client.topic_ack = [] - return initialise_client - -def on_connect(client, userdata, flags, rc): - print("Connected with result code " + str(rc)) - # client.subscribe(mqtt_config["topic"]) - client.subscribe("data/formation") - - -def on_message(client, userdata, msg): - # print(f"got {msg.payload.decode('utf-8')}") - # print(f"msg.topic {msg.payload.decode('utf-8')}") - ros_pub(msg.payload.decode('utf-8')) - - - -if __name__ == '__main__': - # Mqtt - mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/formation"} - - client = initialise_clients("command") - client.on_connect = on_connect - client.on_message = on_message - client.connect(mqtt_config["host"], mqtt_config["port"], 60) - - - - # Ros - Mqtt_Node = 'publisher_message_py' - rospy.init_node(Mqtt_Node) - # initialize Ros node - topicName = 'formation_message' - publisher = rospy.Publisher(topicName,String,queue_size=10) - - rate = rospy.Rate(10) - - - - client.loop_forever() - -# mqtt connect code list -# 0: Connection successful -# 1: Connection refused – incorrect protocol version -# 2: Connection refused – invalid client identifier -# 3: Connection refused – server unavailable -# 4: Connection refused – bad username or password -# 5: Connection refused – not authorised -# 6-255: Currently unused. diff --git a/class_model/src/MQTT_sub_message.py b/class_model/src/MQTT_sub_message.py deleted file mode 100755 index 791a859..0000000 --- a/class_model/src/MQTT_sub_message.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python3 -#coding:utf-8 -import sys -import json -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 - -mqtt_config = {"host": "192.168.50.27", "port": 1883, "topic": "data/formation"} -data ={} -# Ros - -def callBack_message(command): - - dataUpdate = {"message": command.data} - data.update(dataUpdate) - dataJsonFormate = json.dumps(data) - 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): - 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 = 'message_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" - - subscriber = rospy.Subscriber("/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() \ No newline at end of file diff --git a/class_model/src/local_mqtt_pub_data_to_ros.py b/class_model/src/local_mqtt_pub_data_to_ros.py index 0b1b392..f60328b 100755 --- a/class_model/src/local_mqtt_pub_data_to_ros.py +++ b/class_model/src/local_mqtt_pub_data_to_ros.py @@ -5,8 +5,7 @@ import ssl import rospy from std_msgs.msg import String import paho.mqtt.client as mqtt -import json -# import json_transport + # Ros diff --git a/class_model/src/local_mqtt_sub_data_from_ros.py b/class_model/src/local_mqtt_sub_data_from_ros.py index 2f01bcf..dc1b8a7 100755 --- a/class_model/src/local_mqtt_sub_data_from_ros.py +++ b/class_model/src/local_mqtt_sub_data_from_ros.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 #coding:utf-8 import sys -import json +import orjson import paho.mqtt.client as mqtt @@ -41,7 +41,7 @@ def callBack_gps(GPS): alt=int(GPS.altitude*100) dataGpsUpdate = {"lat": lat, "lon": lon, "alt": alt} data.update(dataGpsUpdate) - # dataJsonFormate = json.dumps(data) + # dataJsonFormate = orjson.dumps(data) # mqtt_Pub(message=dataJsonFormate) # print ('lat:'+lat+'\n'+'lon:'+lon+'\n') @@ -50,7 +50,7 @@ def callBack_compass_hdg(Compass): heading = int(Compass.data*100) dataGpsUpdate = {"heading": heading} data.update(dataGpsUpdate) - dataJsonFormate = json.dumps(data) + dataJsonFormate = orjson.dumps(data) mqtt_Pub(message=dataJsonFormate) def callBack_state(state):