remov old python code
RangeOfGlitching
3 years ago
parent
ec4b10b723
commit
1a9f27d08a
@ -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