Merge branch 'protobuf' of github.com:Xuan0319/DroneControl into protobuf
commit
53456c157e
@ -1,49 +0,0 @@
|
||||
import cvzone
|
||||
import cv2
|
||||
import numpy as np
|
||||
import time
|
||||
|
||||
|
||||
class PID:
|
||||
def __init__(self, pidVals, targetVal, axis=0, limit=None):
|
||||
self.pidVals = pidVals
|
||||
self.targetVal = targetVal
|
||||
self.axis = axis
|
||||
self.pError = 0
|
||||
self.limit = limit
|
||||
self.I = 0
|
||||
self.pTime = 0
|
||||
|
||||
def update(self, cVal):
|
||||
# Current Value - Target Value
|
||||
t = time.time() - self.pTime
|
||||
error = cVal - self.targetVal
|
||||
P = self.pidVals[0] * error
|
||||
self.I = self.I + (self.pidVals[1] * error * t)
|
||||
D = (self.pidVals[2] * (error - self.pError)) / t
|
||||
|
||||
result = P + self.I + D
|
||||
|
||||
if self.limit is not None:
|
||||
result = float(np.clip(result, self.limit[0], self.limit[1]))
|
||||
self.pError = error
|
||||
self.ptime = time.time()
|
||||
|
||||
return result
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
# For a 640x480 image center target is 320 and 240
|
||||
xPID = PID([1, 0.000000000001, 1], 640 // 2)
|
||||
yPID = PID([1, 0.000000000001, 1], 480 // 2, axis=1, limit=[-100, 100])
|
||||
|
||||
while True:
|
||||
|
||||
xVal = int(xPID.update(cx))
|
||||
yVal = int(yPID.update(cy))
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,81 +0,0 @@
|
||||
#! /usr/bin/env python3
|
||||
#coding:utf-8
|
||||
|
||||
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 ,publisher1 ,publisher2,publisher3,publisher4,publisher5
|
||||
# data = json.loads(dataJson)
|
||||
publisher.publish(dataJson) #將date字串發布到topic
|
||||
publisher1.publish(dataJson)
|
||||
publisher2.publish(dataJson)
|
||||
publisher3.publish(dataJson)
|
||||
publisher4.publish(dataJson)
|
||||
publisher5.publish(dataJson)
|
||||
publisher6.publish(dataJson)
|
||||
|
||||
rate.sleep()
|
||||
# print(f"publish data {data}")
|
||||
|
||||
# MQTT
|
||||
def on_connect(self, userdata, flags, rc):
|
||||
print("Connected with result code " + str(rc))
|
||||
client.subscribe("cmd/broadcast")
|
||||
|
||||
|
||||
def on_message(self, userdata, msg):
|
||||
#msg = msg.payload.decode('utf-8')
|
||||
print(f"msg.topic {msg}")
|
||||
ros_pub(msg.payload.decode('utf-8'))
|
||||
|
||||
|
||||
def initialise_clients(clientName):
|
||||
# callback assignment
|
||||
initialise_client = mqtt.Client(clientName, False)
|
||||
initialise_client.topic_ack = []
|
||||
return initialise_client
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# Mqtt
|
||||
mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "cmd/broadcast"}
|
||||
client = initialise_clients("receiver")
|
||||
client.on_connect = on_connect
|
||||
client.on_message = on_message
|
||||
client.connect(mqtt_config["host"], mqtt_config["port"], 60)
|
||||
|
||||
# Ros
|
||||
Mqtt_Node = 'publisher_py'
|
||||
rospy.init_node("cmd_receiver")
|
||||
# initialize Ros node
|
||||
topicName = 'cmd_receiver'
|
||||
publisher = rospy.Publisher(topicName,String,queue_size=10)
|
||||
|
||||
topicName1 = '/drone1/cmd_receiver'
|
||||
publisher1 = rospy.Publisher(topicName1,String,queue_size=10)
|
||||
|
||||
topicName2 = '/drone2/cmd_receiver'
|
||||
publisher2 = rospy.Publisher(topicName2,String,queue_size=10)
|
||||
|
||||
topicName3 = '/drone3/cmd_receiver'
|
||||
publisher3 = rospy.Publisher(topicName3,String,queue_size=10)
|
||||
|
||||
topicName4 = '/drone4/cmd_receiver'
|
||||
publisher4 = rospy.Publisher(topicName4,String,queue_size=10)
|
||||
|
||||
topicName5 = '/drone5/cmd_receiver'
|
||||
publisher5 = rospy.Publisher(topicName5,String,queue_size=10)
|
||||
|
||||
topicName6 = '/drone6/cmd_receiver'
|
||||
publisher6 = rospy.Publisher(topicName6,String,queue_size=10)
|
||||
|
||||
|
||||
rate = rospy.Rate(10)
|
||||
|
||||
client.loop_forever()
|
||||
@ -1,16 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
#coding:utf-8
|
||||
import rospy
|
||||
# from mavros_msgs.msg import Waypoint
|
||||
from diagnostic_msgs.msg import KeyValue
|
||||
|
||||
def callBack(data):
|
||||
print (data)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
nodeName = 'subscriber_py'
|
||||
rospy.init_node(nodeName)
|
||||
topicName = '/Flight_Information_reciver'
|
||||
subscriber = rospy.Subscriber(topicName,KeyValue,callBack) #從topic獲取string再呼叫callback
|
||||
rospy.spin()
|
||||
@ -1,54 +0,0 @@
|
||||
#! /usr/bin/env python3
|
||||
#coding:utf-8
|
||||
|
||||
import paho.mqtt.client as mqtt
|
||||
import time
|
||||
|
||||
|
||||
def on_connect(self, userdata, flags, rc):
|
||||
global connect_flag
|
||||
print("Connected with result code " + str(rc))
|
||||
connect_flag = True
|
||||
|
||||
|
||||
|
||||
def initialise_clients(clientName):
|
||||
# callback assignment
|
||||
initialise_client = mqtt.Client(clientName, True)
|
||||
initialise_client.topic_ack = []
|
||||
return initialise_client
|
||||
|
||||
|
||||
# publish a message
|
||||
def publish(topics, message, waitForAck=False):
|
||||
mid = client.publish(topics, message, 1)[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)
|
||||
|
||||
|
||||
connect_flag = False
|
||||
mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"}
|
||||
client = initialise_clients(mqtt_config["name"])
|
||||
client.on_publish = on_publish
|
||||
client.on_connect = on_connect
|
||||
# client.on_message = on_message
|
||||
|
||||
client.connect(mqtt_config["host"], mqtt_config["port"], 60)
|
||||
client.loop_start()
|
||||
|
||||
# publish(topicBroadcast, "Connect", True)
|
||||
while True:
|
||||
if connect_flag:
|
||||
break
|
||||
|
||||
while True:
|
||||
command = input("command: ")
|
||||
publish(mqtt_config["topic"], command)
|
||||
Loading…
Reference in New Issue