from math import atan2
import cv2
import numpy as np
import argparse
import os
import sys
import datetime
from paho.mqtt import client as mqtt_client
import random
import time
import threading
import json
import math

MQTT相关

broker = '127.0.0.1'

broker = 'www.woyilian.com'

port = 1883
odomxyz_topic = "/odom/xyz"
raw_odom_topic ="/sensors/odom"
bno_topic = "/sensors/bno085"

client_id = 'python-mqtt-{}'.format(random.randint(0, 1000))

控制指令topic

cmd_vel_topic = "/cmd/vel"

vr =0
vl =0
vel =0.4
ang =0.1
start_flg =0
glat =0
glnt =0
gpsvelocity =0
ang_bias =0
_gPosition_x =0
_gPosition_y =0
class caculate_baidu_map:
def init(self):
self.lat1 =None
self.lon1 = None
self.lat2 = None
self.lon2 = None

def calculate_distance(self):  
    # 将经纬度转换为弧度  
    _lat1, _lon1, _lat2, _lon2 = map(math.radians, [self.lat1, self.lon1, self.lat2, self.lon2])  
    # 根据Haversine公式计算距离  
    dlon = _lon2 - _lon1  
    dlat = _lat2 - _lat1  
    a = math.sin(dlat/2)**2 + math.cos(_lat1) * math.cos(_lat2) * math.sin(dlon/2)**2  
    c = 2 * math.asin(math.sqrt(a))  
    r = 6371393  # 地球平均半径，单位为meters  
    distance = c * r  
    return distance
def calculate_bearing(self):  
    # 将经纬度转换为弧度  
    _lat1, _lon1, _lat2, _lon2 = map(math.radians, [self.lat1, self.lon1, self.lat2, self.lon2])  
    # 计算两个点在二维平面上的向量  
    dx = _lon2 - _lon1  
    dy = _lat2 - _lat1  
    # 计算该向量的角度  
    bearing = math.atan2(dy, dx)  
    # 将角度转换为0-360度  
    bearing = math.degrees(bearing)  
    # 确保航向角在0-360度之间  
    if bearing < -180:  
        bearing += 360 
    elif bearing > 180:  
        bearing -= 360            
    return bearing    

def conver2wheel(v,w):
global vr,vl
vr = (2v + w10)/2
vl = (2v - w10)/2

def getdatas(x,y):
data = {"x":"%.2f"%(x),"y":"%.2f"%(y)
}
return data
def loop():
global vel,ang,start_flg,glnt,glat,gpsvelocity
last_time =0

init_x =800/2
init_y =800/2
pix_di =0
start_x =0
start_y =0
first_flg =0
g_img =None
g_img = np.ones((800,800), np.uint8)
g_img.fill(0)
geocood = caculate_baidu_map()
f_index = "waypoints.csv"
road_file = "roadpoints.csv"
while True:
    time.sleep(0.1)
    if glat == 0:
        continue
    if(first_flg ==0) :
        first_flg =1
        geocood.lat1 = glat
        geocood.lon1 = glnt
        continue

    #判断是否行驶超过2.5m的误差3m
    geocood.lat2 =glat      
    geocood.lon2 = glnt
    dis = geocood.calculate_distance()
    #print(f"dis:{dis} \n")
    if(dis >= (0.1)):
        #print(str(x) + ' '+ str(y))
        bearing = geocood.calculate_bearing()
        print(f"dis:{dis},bearing:{bearing} \n")
        rad = math.radians(bearing)
        #print(f"rad:{rad} \n")
        img =None
        img = np.ones((800,800), np.uint8)
        img.fill(0)
        pix_di = (dis)/1;
        start_x = (init_x +  (pix_di*math.cos(rad)))*100/100
        start_y = (init_y +  (pix_di*math.sin(rad)))*100/100
        print(str(init_x)+" "+str(init_y)+" "+str(start_x)+" "+str(start_y))
        with open(f_index, "a") as findex:
            findex.write(str(glat)+","+str(glnt)+","+str(start_x)+","+str(start_y)+"\n")
        cv2.line(img,(int(init_x),int(init_y)), (int(start_x),int(start_y)),(255),10)
        cv2.line(g_img,(int(init_x),int(init_y)), (int(start_x),int(start_y)),(255),10)
        #找到临时的道路为白色点位 
        idx = cv2.findNonZero( img )
        #cv2.imshow('img', img)
        #cv2.waitKey(500)
        #byteArr = bytearray(idx)#1xN维的数据
        response_2d = None
        response_2d = idx.reshape(-1, idx.shape[-1])
        with open(road_file, "ab") as f:
            np.savetxt(f, response_2d, fmt='%d',delimiter=',')  # 整数
        jsondata =getdatas(start_x,start_y)
        print(jsondata)
        d = json.dumps(jsondata)
        result = client.publish(roads_topic,d,0)

        status = result[0]
        if status == 0:
            print(f"Send  to topic `{roads_topic}`")
        else:
            print(f"Failed to send message to topic {roads_topic}")
        cv2.imwrite('road.png', g_img)
        geocood.lat1 = glat
        geocood.lon1 = glnt
        init_x = start_x
        init_y = start_y

作为子线程开启

th = threading.Thread(target=loop)
th.setDaemon(True)
th.start()

定义mqtt的client

client = mqtt_client.Client(client_id)
last_time =0
def on_connect(client, userdata, flags, rc):
print("Connected with result code "+str(rc))

client.subscribe("test")

def on_message(client, userdata, msg):
global glat,glnt,ang_bias,first_in,_gPosition_x,_gPosition_y
if (msg.topic == odomxyz_topic):
data = json.loads(msg.payload.decode("utf-8"))
glat = data['x']
glnt = data['y']
gpsvelocity = data['z']
if (msg.topic == raw_odom_topic):
data = json.loads(msg.payload.decode("utf-8"))
glat = data['le']
glnt = data['re']

Compute distance moved by each wheel

    Dl = data['le']*0.1326 *1.1#/mm
    Dr = data['re']*0.1326 *1.1#mm
    Dc = (Dl+Dr)/2.0
    theta  =(Dr-Dl)/200  #theta  车宽度为l=0.2m
    o_x =  Dc *cos(ang*3.1415926/180)#转换成mm
    o_y =  Dc *sin(ang*3.1415926/180)
    _gPosition_x = _gPosition_x +  o_x
    _gPosition_y = _gPosition_y +  o_y
    #gpsvelocity = data['z']
if (msg.topic == bno_topic):
    data = json.loads(msg.payload.decode("utf-8"))
    if first_in ==0：
        first_in =1
        ang_bias = data['yaw']
    else:
        glnt = data['roll']
        ang = data['yaw'] -ang_bias

        if(ang  < -180)  #Note:numpy and OpenCV X and Y reversed
            ang  = ang  + 360
        else if( ang  > 180)
            ang  = ang  - 360 

暂时保留

def publish(client):
print(" datetime")

if name == 'main':
print("gps connected!")
client.on_connect = on_connect
client.on_message = on_message
client.connect(broker, port, 60)

publish(client)

# 订阅来自定位
client.subscribe(odomxyz_topic)
# 订阅来自里程计定位
client.subscribe(raw_odom_topic)
# 订阅来自bno
client.subscribe(bno_topic)

client.loop_forever()