查看: 1686|回复: 1

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

[复制链接]
本帖最后由 question 于 2019-3-29 13:58 编辑

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

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

  •     一切从文档开始:

硬件链接部分:

屏幕快照 2019-03-29 12.39.39.png
看文档就是motor的1脚接到lds的4脚,motor的2脚接到lds的6脚

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

数据部分:
屏幕快照 2019-03-29 12.39.54.png

看到数据长度为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。
[Python] 纯文本查看 复制代码
import os
#[url=mailto:Question_h@sina.com]Question_h@sina.com[/url] 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[0:flagstart]+strlist[flagstart+23:]
            with open('/boot/cmdline.txt','w') as fd:
                fd.writelines(strlists)
                fd.close()
                return True
    return False

isConfig()
execCmd("sudo systemctl stop [url=mailto:serial-getty@ttyAMA0.service]serial-getty@ttyAMA0.service[/url]")
execCmd("sudo systemctl disable [url=mailto:serial-getty@ttyAMA0.service]serial-getty@ttyAMA0.service[/url]")
isCmdline()



3.雷达数据接收并且通过tcp传到笔记本进行画图
下面是树莓派端代码(python3):
[Python] 纯文本查看 复制代码
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[0]==0xfa: #找到数据头
                distlist=[]
                ldtmp=ldser.read(41)
                #print(binascii.b2a_hex(ldtmp))
                degree=(ldtmp[0]-0xa0)*6
                for i in range(0,6):
                    distdict={}
                    #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]))
                    distdict["degree"]=degree+i
                    distdict["dist"]=ldtmp[6+6*i]*0xff+ldtmp[5+6*i]
                    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[1:], "-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,退出报错,大家自己改着玩吧。
[Python] 纯文本查看 复制代码
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 = [r_out*np.cos(theta[i]) for i in range(len(theta))]
    #y_out = [r_out*np.sin(theta[i]) for i in range(len(theta))]
    #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[0:jsonflag+1])
                    lddata.put(json.loads(datatmp[0:jsonflag+1]))
                    datatmp=datatmp[jsonflag+1:]
            except Exception as e:
                #print('28',datatmp[0:jsonflag+1])
                print (e,str(jsonflag),datatmp[0:jsonflag+1])
            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["degree"])]=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()




5.效果图 lidar.jpeg

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

yoyojacky  初级技匠

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

大神,请收下我的膝盖~哈哈, ROS 的坑很深,慢慢填。
截图201903291446306394.png
回复

使用道具 举报

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

本版积分规则

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

硬件清单

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

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

mail