设为首页收藏本站
首页 > 论坛 > 机器人

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

question | 初级技师 | 创造力： | 帖子： | 发消息 | 串个门 | 加好友 | 打招呼
2019-3-29 13:58:54 [显示全部楼层]
3731浏览
1回复
1收藏
查看: 3731|回复: 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. 

  4. def execCmd(cmdstr):

  5.     pipeline = os.popen(cmdstr)

  6.     print(pipeline.read())

  7. 

  8. def isConfig():

  9.     with open('/boot/config.txt','r+') as fd:

  10.         linetmp=fd.readline()

  11.         while linetmp:

  12.             if linetmp=="dtoverlay=pi3-miniuart-bt":

  13.                 print('config ok')

  14.                 fd.close()

  15.                 return True

  16.             linetmp=fd.readline()

  17.         fd.write("linetmp=fd.readline()")

  18.         fd.close()

  19.     return False

  20. 

  21. def isCmdline():

  22.     strlists=None

  23.     with open('/boot/cmdline.txt','r+') as fd:

  24.         strlists=fd.readlines()

  25.         fd.close()

  26.     for i in range(len(strlists)):

  27.         flagstart=strlists<i>.find("console=serial0,115200 ")

  28.         if flagstart:

  29.             strlist=strlists<i>

  30.             print('cmdline ok')

  31.             strlists<i>=strlist[0:flagstart]+strlist[flagstart+23:]

  32.             with open('/boot/cmdline.txt','w') as fd:

  33.                 fd.writelines(strlists)

  34.                 fd.close()

  35.                 return True

  36.     return False

  37. 

  38. isConfig()

  39. execCmd("sudo systemctl stop <a href="mailto:serial-getty@ttyAMA0.service" target="_blank">serial-getty@ttyAMA0.service</a>")

  40. execCmd("sudo systemctl disable <a href="mailto:serial-getty@ttyAMA0.service" target="_blank">serial-getty@ttyAMA0.service</a>")

  41. 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. 

  17. def tcpcli(serip,serport):

  18.     print(serip,serport)

  19.     try:

  20.         sockfs=socket.socket(socket.AF_INET, socket.SOCK_STREAM)

  21.         sockfs.connect((serip,int(serport)))

  22.     except socket.error as msg:

  23.         print(msg)

  24.         sockfs.close() 

  25.     return sockfs

  26. 

  27. def senddata(serip,serport,lddata):

  28.     print('senddata')

  29.     sockfd=tcpcli(serip,serport)

  30.     while(True):

  31.         try:

  32.             datatmp = lddata.get(timeout=5.0)

  33.             sockfd.send((json.dumps(datatmp)).encode('utf-8'))

  34.         except queue.Empty:

  35.             continue

  36.         except socket.error as msg:

  37.             print(msg)

  38.             sockfd.close()

  39.             sockfd=tcpcli(serip,serport)

  40.             continue

  41.     sockfs.close()

  42. 

  43. 

  44. def openser(device,lddata):

  45.     print('openser')

  46.     datalist=[]

  47.     datadict={}

  48.     ldser=serial.Serial(device, 230400, timeout=1)

  49.     ldser.write(b'b')

  50.     i=0

  51.     #for i in range(0,60):

  52.     while(True):

  53.         if ldread:

  54.             sync=ldser.read()

  55.             if sync and sync[0]==0xfa: #找到数据头

  56.                 distlist=[]

  57.                 ldtmp=ldser.read(41)

  58.                 #print(binascii.b2a_hex(ldtmp))

  59.                 degree=(ldtmp[0]-0xa0)*6

  60.                 for i in range(0,6):

  61.                     distdict={}

  62.                     #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]))

  63.                     distdict["degree"]=degree+i

  64.                     distdict["dist"]=ldtmp[6+6*i]*0xff+ldtmp[5+6*i]

  65.                     distlist.append(distdict)

  66.                 datadict['data']=distlist

  67.                 #print(datadict)

  68.                 lddata.put(datadict)

  69.                 #sockfd.send((json.dumps(datadict)).encode('utf-8'))

  70. 

  71.                 

  72.             else:

  73.                 i=i+1

  74.                 if i>60:

  75.                     print("can't find sync,drop data",sync,type(sync))

  76.                     #lddata.put(i)

  77.                     i=0

  78.         else:

  79.             ldser.write(b'e')

  80.     ldser.close()

  81.     #lddata.put("1")

  82.     

  83. 

  84. def usage():

  85.     print("lider 1.0.0 - (C) 2019 Question\n\n-d device\n-h hostnma\n-p tcp port\n")

  86. 

  87. def main():

  88.     serip='127.0.0.1'

  89.     serport=80

  90.     device=None

  91.     try:

  92.         opts, args = getopt.getopt(sys.argv[1:], "-d:-h:-p:",["help","version"])

  93.         for op, value in opts:

  94.             if op == "-d":

  95.                 device = value

  96.             elif op == "-h":

  97.                 #hostname = value

  98.                 serip=value

  99.             elif op == "-p":

  100.                 serport = value

  101.     except getopt.GetoptError:

  102.         usage()

  103.         sys.exit()

  104.     threads = []

  105.     t1 = threading.Thread(target=openser,args=(device,lddata))

  106.     threads.append(t1)

  107.     t2 = threading.Thread(target=senddata,args=(serip,serport,lddata))

  108.     threads.append(t2)

  109.     for t in threads:

  110.         t.setDaemon(True)

  111.         t.start()

  112.     t.join()

  113.     #time.sleep(10)

  114.     #sockfd=tcpcli(serip,serport)

  115.     #openser(sockfd,device)   

  116.     print("serial:%s,hostname:%s,tcp port:%s"%(device,serip,serport))

  117.     #lddata.get()

  118. 

  119. if __name__ == '__main__':

  120.   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. 

  12. carsocket=[]

  13. lddata = queue.Queue()

  14. distdict={}

  15. dot={}

  16. fig = plt.figure(figsize=(8, 8)) #设置画布 600*600 貌似不设置会变形

  17. ax = fig.add_subplot(111)

  18. ax.set(xlim=[-4000, 4000], ylim=[-4000, 4000], title='lidar',

  19.        ylabel='Y-Axis', xlabel='X-Axis')

  20. 

  21. for i in range(0,360):

  22.     dot<i>, = ax.plot([], [],'b,')

  23. 

  24. def init():

  25.     #ax.set_xlim(-2, 2)

  26.     #ax.set_ylim(-2, 2)

  27.     plt.scatter(0, 0, color='red', marker='+')

  28.     #x_out = [r_out*np.cos(theta<i>) for i in range(len(theta))]

  29.     #y_out = [r_out*np.sin(theta<i>) for i in range(len(theta))]

  30.     #ln1.set_data(x_out, y_out)

  31.     return None

  32. 

  33. 

  34. def update(i):

  35.     global distdict

  36.     for i in range(0,360):

  37.         xp=int(distdict<i>)*math.sin(math.radians(i))

  38.         yp=int(distdict<i>)*math.cos(math.radians(i))

  39.         dot<i>.set_data(xp,yp)

  40.         #plt.scatter(xp, yp, color='black', marker='.')

  41. 

  42.     return 

  43. 

  44. 

  45. 

  46. def carser(port):

  47.     datatmp=''

  48.     csok=socket.socket(socket.AF_INET,socket.SOCK_STREAM,0)

  49.     csok.bind(('0.0.0.0',port))

  50.     csok.listen(5)

  51.     print("小车接收数据socket进入监听:")

  52.     while True:

  53.         clients,addr = csok.accept()      

  54.         print("小车发送，地址: %s" % str(addr))

  55.         #carsocket.append(clients)

  56. 

  57.         while clients:

  58.             try:

  59.                 datatmp=datatmp+clients.recv(100).decode()

  60.                 jsonflag=datatmp.find("}{"data":")

  61.                 if jsonflag >0:

  62.                     #print(datatmp[0:jsonflag+1])

  63.                     lddata.put(json.loads(datatmp[0:jsonflag+1]))

  64.                     datatmp=datatmp[jsonflag+1:]

  65.             except Exception as e:

  66.                 #print('28',datatmp[0:jsonflag+1])

  67.                 print (e,str(jsonflag),datatmp[0:jsonflag+1])

  68.             except socket.error as msg:

  69.                 break

  70.             except KeyboardInterrupt:

  71.                 print("KeyboardInterrupt---")

  72.                 break

  73.     csok.close()

  74. 

  75. 

  76. def handledata(maxx,maxy):

  77.     global distdict

  78.     while True:

  79.         try:

  80.             datatmp = lddata.get(timeout=5.0)

  81.             datalist=datatmp['data']

  82.             print(distdict)

  83.             for dl in datalist:

  84.                 distdict[int(dl["degree"])]=int(dl["dist"])

  85.             #print(type(datatmp))

  86.         except queue.Empty:

  87.             continue

  88. 

  89. 

  90. 

  91. 

  92. 

  93. def main():

  94.     

  95.     threads = []

  96.     t1 = threading.Thread(target=carser,args=(8000,))

  97.     threads.append(t1)

  98.     t2 = threading.Thread(target=handledata,args=(4000,4000))

  99.     threads.append(t2)

  100. 

  101.     for t in threads:

  102.         t.setDaemon(True)

  103.         t.start()

  104.     

  105.     plt.scatter(0, 0, color='red', marker='+')

  106.     ani = animation.FuncAnimation(fig, update, 100, init_func=init, interval=10)

  107.     #ani.save('roll.gif', writer='imagemagick', fps=100)

  108. 

  109.     plt.show()

  110.     '''

  111.     t.join()

  112.     print ("退出主线程")

  113.     '''

  114. if __name__ == '__main__':

  115.     main()</i></i></i></i></i></i>
复制代码
5.效果图蘑菇云无人小车1-激光雷达图3

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


回复

使用道具 举报

精彩评论(1)

yoyojacky  初级技匠

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

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

使用道具 举报

高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
撰写新帖
关于楼主

楼主的其它帖子

background 创造力兑换 background 心愿单 background 创客导航
上海智位机器人股份有限公司 沪ICP备09038501号-4

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

mail