3299浏览
查看: 3299|回复: 1

[项目] 蘑菇云无人小车1-激光雷达

[复制链接]
  • 项目简介:
             会员们脑洞打开说要搞个无人小车,目标自动环汇智湖,捡钱,夏天吹妹子裙底。
              坑就这样挖好了,第一个实验版本就先室内搞起吧。
             经过群里的吵架,项目就开始。第一个版本先在室内跑。
  • 硬件平台
             激光雷达(闲鱼二手货,土豪可以在df商城买那个好用,资料齐全,直接支持ros)
             树莓派3b+(这里有坑 Ubuntu mate不支持 ros安装不方便)
             减速电机 机器人底盘(还没开始 没型号推荐哈哈)

  • 填坑开始(激光雷达部分)

  •     一切从文档开始:

硬件链接部分:

蘑菇云无人小车1-激光雷达图1
看文档就是motor的1脚接到lds的4脚,motor的2脚接到lds的6脚

然后整个激光雷达就是 :6接5v ,3接gnc ,5和2接串口。

数据部分:
蘑菇云无人小车1-激光雷达图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。
  1. import os
  2. #<a href="mailto:Question_h@sina.com" target="_blank">Question_h@sina.com</a> 2019.03.18
  3. def execCmd(cmdstr):
  4.     pipeline = os.popen(cmdstr)
  5.     print(pipeline.read())
  6. def isConfig():
  7.     with open('/boot/config.txt','r+') as fd:
  8.         linetmp=fd.readline()
  9.         while linetmp:
  10.             if linetmp=="dtoverlay=pi3-miniuart-bt":
  11.                 print('config ok')
  12.                 fd.close()
  13.                 return True
  14.             linetmp=fd.readline()
  15.         fd.write("linetmp=fd.readline()")
  16.         fd.close()
  17.     return False
  18. def isCmdline():
  19.     strlists=None
  20.     with open('/boot/cmdline.txt','r+') as fd:
  21.         strlists=fd.readlines()
  22.         fd.close()
  23.     for i in range(len(strlists)):
  24.         flagstart=strlists<i>.find("console=serial0,115200 ")
  25.         if flagstart:
  26.             strlist=strlists<i>
  27.             print('cmdline ok')
  28.             strlists<i>=strlist[0:flagstart]+strlist[flagstart+23:]
  29.             with open('/boot/cmdline.txt','w') as fd:
  30.                 fd.writelines(strlists)
  31.                 fd.close()
  32.                 return True
  33.     return False
  34. isConfig()
  35. execCmd("sudo systemctl stop <a href="mailto:serial-getty@ttyAMA0.service" target="_blank">serial-getty@ttyAMA0.service</a>")
  36. execCmd("sudo systemctl disable <a href="mailto:serial-getty@ttyAMA0.service" target="_blank">serial-getty@ttyAMA0.service</a>")
  37. isCmdline()</i></i></i>
复制代码

3.雷达数据接收并且通过tcp传到笔记本进行画图

下面是树莓派端代码(python3):

  1. import sys,getopt,time
  2. import serial
  3. import queue
  4. #from queue import Queue
  5. import socket
  6. import signal
  7. import json
  8. import threading
  9. import binascii
  10. #BaudRate 230400
  11. ldread = True
  12. lddata = queue.Queue()
  13. #sockfs=None
  14. #serip='127.0.0.1'
  15. #serport = 80
  16. def tcpcli(serip,serport):
  17.     print(serip,serport)
  18.     try:
  19.         sockfs=socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  20.         sockfs.connect((serip,int(serport)))
  21.     except socket.error as msg:
  22.         print(msg)
  23.         sockfs.close()
  24.     return sockfs
  25. def senddata(serip,serport,lddata):
  26.     print('senddata')
  27.     sockfd=tcpcli(serip,serport)
  28.     while(True):
  29.         try:
  30.             datatmp = lddata.get(timeout=5.0)
  31.             sockfd.send((json.dumps(datatmp)).encode('utf-8'))
  32.         except queue.Empty:
  33.             continue
  34.         except socket.error as msg:
  35.             print(msg)
  36.             sockfd.close()
  37.             sockfd=tcpcli(serip,serport)
  38.             continue
  39.     sockfs.close()
  40. def openser(device,lddata):
  41.     print('openser')
  42.     datalist=[]
  43.     datadict={}
  44.     ldser=serial.Serial(device, 230400, timeout=1)
  45.     ldser.write(b'b')
  46.     i=0
  47.     #for i in range(0,60):
  48.     while(True):
  49.         if ldread:
  50.             sync=ldser.read()
  51.             if sync and sync[0]==0xfa: #找到数据头
  52.                 distlist=[]
  53.                 ldtmp=ldser.read(41)
  54.                 #print(binascii.b2a_hex(ldtmp))
  55.                 degree=(ldtmp[0]-0xa0)*6
  56.                 for i in range(0,6):
  57.                     distdict={}
  58.                     #print("degree:%d,dist:%d,data[%d]:%x,data[%d]:%x"%(degree+i,ldtmp[6+6*i]*0xff+ldtmp[5+6*i],5+6*i,ldtmp[5+6*i],6+6*i,ldtmp[6+6*i]))
  59.                     distdict["degree"]=degree+i
  60.                     distdict["dist"]=ldtmp[6+6*i]*0xff+ldtmp[5+6*i]
  61.                     distlist.append(distdict)
  62.                 datadict['data']=distlist
  63.                 #print(datadict)
  64.                 lddata.put(datadict)
  65.                 #sockfd.send((json.dumps(datadict)).encode('utf-8'))
  66.                
  67.             else:
  68.                 i=i+1
  69.                 if i>60:
  70.                     print("can't find sync,drop data",sync,type(sync))
  71.                     #lddata.put(i)
  72.                     i=0
  73.         else:
  74.             ldser.write(b'e')
  75.     ldser.close()
  76.     #lddata.put("1")
  77.    
  78. def usage():
  79.     print("lider 1.0.0 - (C) 2019 Question\n\n-d device\n-h hostnma\n-p tcp port\n")
  80. def main():
  81.     serip='127.0.0.1'
  82.     serport=80
  83.     device=None
  84.     try:
  85.         opts, args = getopt.getopt(sys.argv[1:], "-d:-h:-p:",["help","version"])
  86.         for op, value in opts:
  87.             if op == "-d":
  88.                 device = value
  89.             elif op == "-h":
  90.                 #hostname = value
  91.                 serip=value
  92.             elif op == "-p":
  93.                 serport = value
  94.     except getopt.GetoptError:
  95.         usage()
  96.         sys.exit()
  97.     threads = []
  98.     t1 = threading.Thread(target=openser,args=(device,lddata))
  99.     threads.append(t1)
  100.     t2 = threading.Thread(target=senddata,args=(serip,serport,lddata))
  101.     threads.append(t2)
  102.     for t in threads:
  103.         t.setDaemon(True)
  104.         t.start()
  105.     t.join()
  106.     #time.sleep(10)
  107.     #sockfd=tcpcli(serip,serport)
  108.     #openser(sockfd,device)   
  109.     print("serial:%s,hostname:%s,tcp port:%s"%(device,serip,serport))
  110.     #lddata.get()
  111. if __name__ == '__main__':
  112.   main()
复制代码
运行命令:python3 lidar.py -d /dev/ttyUSB0 -h 172.16.201.93 -p 20002
4.笔记本用 matplotlib显示点图,第一次用matplotlib 写的不好 还有bug,退出报错,大家自己改着玩吧。
  1. import socket
  2. import threading
  3. import time
  4. import queue
  5. import json
  6. import matplotlib.pyplot as plt
  7. #from matplotlib.animation import FuncAnimation
  8. import matplotlib.animation as animation
  9. import math
  10. import numpy as np
  11. carsocket=[]
  12. lddata = queue.Queue()
  13. distdict={}
  14. dot={}
  15. fig = plt.figure(figsize=(8, 8)) #设置画布 600*600 貌似不设置会变形
  16. ax = fig.add_subplot(111)
  17. ax.set(xlim=[-4000, 4000], ylim=[-4000, 4000], title='lidar',
  18.        ylabel='Y-Axis', xlabel='X-Axis')
  19. for i in range(0,360):
  20.     dot<i>, = ax.plot([], [],'b,')
  21. def init():
  22.     #ax.set_xlim(-2, 2)
  23.     #ax.set_ylim(-2, 2)
  24.     plt.scatter(0, 0, color='red', marker='+')
  25.     #x_out = [r_out*np.cos(theta<i>) for i in range(len(theta))]
  26.     #y_out = [r_out*np.sin(theta<i>) for i in range(len(theta))]
  27.     #ln1.set_data(x_out, y_out)
  28.     return None
  29. def update(i):
  30.     global distdict
  31.     for i in range(0,360):
  32.         xp=int(distdict<i>)*math.sin(math.radians(i))
  33.         yp=int(distdict<i>)*math.cos(math.radians(i))
  34.         dot<i>.set_data(xp,yp)
  35.         #plt.scatter(xp, yp, color='black', marker='.')
  36.     return
  37. def carser(port):
  38.     datatmp=''
  39.     csok=socket.socket(socket.AF_INET,socket.SOCK_STREAM,0)
  40.     csok.bind(('0.0.0.0',port))
  41.     csok.listen(5)
  42.     print("小车接收数据socket进入监听:")
  43.     while True:
  44.         clients,addr = csok.accept()      
  45.         print("小车发送,地址: %s" % str(addr))
  46.         #carsocket.append(clients)
  47.         while clients:
  48.             try:
  49.                 datatmp=datatmp+clients.recv(100).decode()
  50.                 jsonflag=datatmp.find("}{"data":")
  51.                 if jsonflag >0:
  52.                     #print(datatmp[0:jsonflag+1])
  53.                     lddata.put(json.loads(datatmp[0:jsonflag+1]))
  54.                     datatmp=datatmp[jsonflag+1:]
  55.             except Exception as e:
  56.                 #print('28',datatmp[0:jsonflag+1])
  57.                 print (e,str(jsonflag),datatmp[0:jsonflag+1])
  58.             except socket.error as msg:
  59.                 break
  60.             except KeyboardInterrupt:
  61.                 print("KeyboardInterrupt---")
  62.                 break
  63.     csok.close()
  64. def handledata(maxx,maxy):
  65.     global distdict
  66.     while True:
  67.         try:
  68.             datatmp = lddata.get(timeout=5.0)
  69.             datalist=datatmp['data']
  70.             print(distdict)
  71.             for dl in datalist:
  72.                 distdict[int(dl["degree"])]=int(dl["dist"])
  73.             #print(type(datatmp))
  74.         except queue.Empty:
  75.             continue
  76. def main():
  77.    
  78.     threads = []
  79.     t1 = threading.Thread(target=carser,args=(8000,))
  80.     threads.append(t1)
  81.     t2 = threading.Thread(target=handledata,args=(4000,4000))
  82.     threads.append(t2)
  83.     for t in threads:
  84.         t.setDaemon(True)
  85.         t.start()
  86.    
  87.     plt.scatter(0, 0, color='red', marker='+')
  88.     ani = animation.FuncAnimation(fig, update, 100, init_func=init, interval=10)
  89.     #ani.save('roll.gif', writer='imagemagick', fps=100)
  90.     plt.show()
  91.     '''
  92.     t.join()
  93.     print ("退出主线程")
  94.     '''
  95. if __name__ == '__main__':
  96.     main()</i></i></i></i></i></i>
复制代码
5.效果图蘑菇云无人小车1-激光雷达图3

十字是激光雷达的位置 红色圈是墙,箭头是我的椅子哈哈


yoyojacky  初级技匠

发表于 2019-3-29 14:47:47

大神,请收下我的膝盖~哈哈, ROS 的坑很深,慢慢填。
蘑菇云无人小车1-激光雷达图1
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2024 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail