蘑菇云无人小车1-激光雷达
[*]项目简介：
会员们脑洞打开说要搞个无人小车，目标自动环汇智湖，捡钱，夏天吹妹子裙底。
坑就这样挖好了，第一个实验版本就先室内搞起吧。
经过群里的吵架，项目就开始。第一个版本先在室内跑。
[*]硬件平台
激光雷达（闲鱼二手货，土豪可以在df商城买那个好用，资料齐全，直接支持ros）
树莓派3b+(这里有坑 Ubuntu mate不支持 ros安装不方便)
减速电机 机器人底盘（还没开始 没型号推荐哈哈）
[*]填坑开始（激光雷达部分）
[*] 一切从文档开始：
硬件链接部分：
看文档就是motor的1脚接到lds的4脚，motor的2脚接到lds的6脚
然后整个激光雷达就是 ：6接5v ，3接gnc ，5和2接串口。
数据部分：
看到数据长度为42bytes
0 为sync就是同步头，固定值为0xfa
1 是degree 换算算法是 angle=angle index*6+angle offset，angleindex的取值范围是：0xA0～0xDB （也就是0～60）因为每组数据包含6组距离 所以 60*6正好是360度的数 据。
2、3 是转速
4～9 是第一组数据 前面4、5是强度，6、7 是距离，8、9是保留，这里的数据要注意大小端。
40、41 是checksum
2.激光雷达与树莓派接线说明
树莓派 LDS
4 vcc
6 gnd
8 TX
10RX
树莓派开启串口需要进行设置，串口开启后 蓝牙就失效了，大家自己看着选择吧，想保留蓝牙就用usb转ttl线。
下面是开启串口的py脚本，写的懒，不接受review，python3。
import os
#<a href="mailto:Question_h@sina.com" target="_blank">Question_h@sina.com</a> 2019.03.18
def execCmd(cmdstr):
pipeline = os.popen(cmdstr)
print(pipeline.read())
def isConfig():
with open('/boot/config.txt','r+') as fd:
linetmp=fd.readline()
while linetmp:
if linetmp=="dtoverlay=pi3-miniuart-bt":
print('config ok')
fd.close()
return True
linetmp=fd.readline()
fd.write("linetmp=fd.readline()")
fd.close()
return False
def isCmdline():
strlists=None
with open('/boot/cmdline.txt','r+') as fd:
strlists=fd.readlines()
fd.close()
for i in range(len(strlists)):
flagstart=strlists<i>.find("console=serial0,115200 ")
if flagstart:
strlist=strlists<i>
print('cmdline ok')
strlists<i>=strlist+strlist
with open('/boot/cmdline.txt','w') as fd:
fd.writelines(strlists)
fd.close()
return True
return False
isConfig()
execCmd("sudo systemctl stop <a href="mailto:serial-getty@ttyAMA0.service" target="_blank">serial-getty@ttyAMA0.service</a>")
execCmd("sudo systemctl disable <a href="mailto:serial-getty@ttyAMA0.service" target="_blank">serial-getty@ttyAMA0.service</a>")
isCmdline()</i></i></i>
3.雷达数据接收并且通过tcp传到笔记本进行画图
下面是树莓派端代码（python3）：
import sys,getopt,time
import serial
import queue
#from queue import Queue
import socket
import signal
import json
import threading
import binascii
#BaudRate 230400
ldread = True
lddata = queue.Queue()
#sockfs=None
#serip='127.0.0.1'
#serport = 80
def tcpcli(serip,serport):
print(serip,serport)
try:
sockfs=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sockfs.connect((serip,int(serport)))
except socket.error as msg:
print(msg)
sockfs.close()
return sockfs
def senddata(serip,serport,lddata):
print('senddata')
sockfd=tcpcli(serip,serport)
while(True):
try:
datatmp = lddata.get(timeout=5.0)
sockfd.send((json.dumps(datatmp)).encode('utf-8'))
except queue.Empty:
continue
except socket.error as msg:
print(msg)
sockfd.close()
sockfd=tcpcli(serip,serport)
continue
sockfs.close()
def openser(device,lddata):
print('openser')
datalist=[]
datadict={}
ldser=serial.Serial(device, 230400, timeout=1)
ldser.write(b'b')
i=0
#for i in range(0,60):
while(True):
if ldread:
sync=ldser.read()
if sync and sync==0xfa: #找到数据头
distlist=[]
ldtmp=ldser.read(41)
#print(binascii.b2a_hex(ldtmp))
degree=(ldtmp-0xa0)*6
for i in range(0,6):
distdict={}
#print("degree:%d,dist:%d,data[%d]:%x,data[%d]:%x"%(degree+i,ldtmp*0xff+ldtmp,5+6*i,ldtmp,6+6*i,ldtmp))
distdict["degree"]=degree+i
distdict["dist"]=ldtmp*0xff+ldtmp
distlist.append(distdict)
datadict['data']=distlist
#print(datadict)
lddata.put(datadict)
#sockfd.send((json.dumps(datadict)).encode('utf-8'))
else:
i=i+1
if i>60:
print("can't find sync,drop data",sync,type(sync))
#lddata.put(i)
i=0
else:
ldser.write(b'e')
ldser.close()
#lddata.put("1")
def usage():
print("lider 1.0.0 - (C) 2019 Question\n\n-d device\n-h hostnma\n-p tcp port\n")
def main():
serip='127.0.0.1'
serport=80
device=None
try:
opts, args = getopt.getopt(sys.argv, "-d:-h:-p:",["help","version"])
for op, value in opts:
if op == "-d":
device = value
elif op == "-h":
#hostname = value
serip=value
elif op == "-p":
serport = value
except getopt.GetoptError:
usage()
sys.exit()
threads = []
t1 = threading.Thread(target=openser,args=(device,lddata))
threads.append(t1)
t2 = threading.Thread(target=senddata,args=(serip,serport,lddata))
threads.append(t2)
for t in threads:
t.setDaemon(True)
t.start()
t.join()
#time.sleep(10)
#sockfd=tcpcli(serip,serport)
#openser(sockfd,device)
print("serial:%s,hostname:%s,tcp port:%s"%(device,serip,serport))
#lddata.get()
if __name__ == '__main__':
main()运行命令：python3 lidar.py -d /dev/ttyUSB0 -h 172.16.201.93 -p 20002
4.笔记本用 matplotlib显示点图，第一次用matplotlib 写的不好 还有bug，退出报错，大家自己改着玩吧。
import socket
import threading
import time
import queue
import json
import matplotlib.pyplot as plt
#from matplotlib.animation import FuncAnimation
import matplotlib.animation as animation
import math
import numpy as np
carsocket=[]
lddata = queue.Queue()
distdict={}
dot={}
fig = plt.figure(figsize=(8, 8)) #设置画布 600*600 貌似不设置会变形
ax = fig.add_subplot(111)
ax.set(xlim=[-4000, 4000], ylim=[-4000, 4000], title='lidar',
ylabel='Y-Axis', xlabel='X-Axis')
for i in range(0,360):
dot<i>, = ax.plot([], [],'b,')
def init():
#ax.set_xlim(-2, 2)
#ax.set_ylim(-2, 2)
plt.scatter(0, 0, color='red', marker='+')
#x_out =
#y_out =
#ln1.set_data(x_out, y_out)
return None
def update(i):
global distdict
for i in range(0,360):
xp=int(distdict<i>)*math.sin(math.radians(i))
yp=int(distdict<i>)*math.cos(math.radians(i))
dot<i>.set_data(xp,yp)
#plt.scatter(xp, yp, color='black', marker='.')
return
def carser(port):
datatmp=''
csok=socket.socket(socket.AF_INET,socket.SOCK_STREAM,0)
csok.bind(('0.0.0.0',port))
csok.listen(5)
print("小车接收数据socket进入监听:")
while True:
clients,addr = csok.accept()
print("小车发送，地址: %s" % str(addr))
#carsocket.append(clients)
while clients:
try:
datatmp=datatmp+clients.recv(100).decode()
jsonflag=datatmp.find("}{\"data\":")
if jsonflag >0:
#print(datatmp)
lddata.put(json.loads(datatmp))
datatmp=datatmp
except Exception as e:
#print('28',datatmp)
print (e,str(jsonflag),datatmp)
except socket.error as msg:
break
except KeyboardInterrupt:
print("KeyboardInterrupt---")
break
csok.close()
def handledata(maxx,maxy):
global distdict
while True:
try:
datatmp = lddata.get(timeout=5.0)
datalist=datatmp['data']
print(distdict)
for dl in datalist:
distdict)]=int(dl["dist"])
#print(type(datatmp))
except queue.Empty:
continue
def main():
threads = []
t1 = threading.Thread(target=carser,args=(8000,))
threads.append(t1)
t2 = threading.Thread(target=handledata,args=(4000,4000))
threads.append(t2)
for t in threads:
t.setDaemon(True)
t.start()
plt.scatter(0, 0, color='red', marker='+')
ani = animation.FuncAnimation(fig, update, 100, init_func=init, interval=10)
#ani.save('roll.gif', writer='imagemagick', fps=100)
plt.show()
'''
t.join()
print ("退出主线程")
'''
if __name__ == '__main__':
main()</i></i></i></i></i></i>5.效果图
十字是激光雷达的位置 红色圈是墙，箭头是我的椅子哈哈
大神，请收下我的膝盖~哈哈， ROS 的坑很深，慢慢填。
页:
[1]