基于DragonBoard 410c的家庭智能环保卫士——（5）获取kobuki位置信息

前面已经对基本方案和相关的算法进行了简单介绍，本期blog，将教大家如何快速的获取kobuki机器人的位置信息，由于之前没有研究过ROS，因此，在使用kobuki的时候，只是需要使用kobuki机器人底盘的功能，能够实现对机器人的控制和获取位置信息即可，如果大家想详细的了解kobuki机器人和ROS操作系统，可以到其官网上查询相关资料，在本期blog中将不进行讨论，只是获取我们需要的信息即可。



在http://blog.csdn.net/andymfc/article/details/56671951 blog中已经教大家如何快速的实现对kobuki机器人进行简单的控制，后续也介绍了一种利用kobuki机器人驱动自带的按键控制机器人demo程序来模拟按键控制机器人，这里我们进一步利用该demo来教大家如何通过运行该demo来实时获取机器人的位置信息。

这个demo在运行的过程中会在终端输出kobuki机器人的位置信息，这里我们就可以利用管道重定向将程序输出从终端重定向到我们写的getLocationLocal.py程序中，然后在该程序中获取位置信息，具体代码如下：

#coding=utf-8

import time

import sys

from locationDB import locationDB

maxA=3.2

minA=-3.2

class getRobotLocation:

def __init__(self):

self.currentLocation=""

self.currentX=0.0

self.currentY=0.0

self.currentA=0.0

self.locReport=""

self.flag=0

def startGetThread(self):

print("start get Thread")

while 1:

self.locReport=sys.stdin.readline()

print(self.locReport)

if self.locReport=="current pose: [0, 0, 0]\n":

self.flag=1

if self.flag==1:

self.currentLocation=(self.locReport[15:-2]).split(', ',2)

print(self.currentLocation[0])

print(self.currentLocation[1])

print(self.currentLocation[2])

self.currentX=float(self.currentLocation[0])

self.currentY=float(self.currentLocation[1])

self.currentA=float(self.currentLocation[2])

if self.currentA>0:

self.currentA=self.currentA*180/3.1415926

else:

self.currentA=360+self.currentA*180/3.1415926

print(self.currentLocation)

print(type(self.currentLocation))

LDB=locationDB("./locationDB.db")

LDB.updateCurrentLocation(self.currentX,self.currentY,self.currentA)

def getCurrentLocation(self):

time.sleep(1)

self.currentLocation=sys.stdin.readline()

return self.currentLocation

if __name__ == '__main__':

robotLocation=getRobotLocation()

location=robotLocation.startGetThread()

#print(location)

同时在上述代码中个，还讲位置信息保存到数据库中，数据库是基于sqlite3 数据库构建的数据库，下面是数据库脚本代码：

#coding=utf-8

import sqlite3

class locationDB:

def __init__(self,dbPath):

self.DB=sqlite3.connect(dbPath)

self.cu=self.DB.cursor()

self.DB.execute("")

self.initTable()

def initTable(self):

try:

#create pushInfo table

self.DB.execute("create table locationInfo(infoID integer,currentX float,currentY float,currentA float)")

self.DB.execute("insert into locationInfo values(0,0.0,0.0,0.0)")

self.DB.commit()

print("creat current location info successful")

except:

print("table is already create")

def getCurrentLocation(self):

querySQL="select currentX ,currentY,currentA from locationInfo where infoID==0"

self.cu.execute(querySQL)

result=self.cu.fetchone()

if result==None:

return None

else:

return (result[0],result[1],result[2])

print(result)

def updateCurrentLocation(self,x,y,a):

format="update locationInfo set currentX=%f ,currentY=%f,currentA=%f where infoID ==0"

values=(x,y,a)

updateSQL=format % values

self.cu.execute(updateSQL)

self.DB.commit()

def __del__(self):

self.DB.commit()

self.DB.close()

最后，在启动kobuki机器人键盘控制demo的过程中编写了一个脚本，以便于直接启动demo，脚本代码如下：

#!/bin/bash

export LD_LIBRARY_PATH=/home/linaro/work/kobuik_core/install/lib

/home/linaro/work/kobuik_core/install/lib/kobuki_driver/simple_keyop

#python ./test.py

最后我们将三个代码分别保存为getLocationLocal.py、locationDB.py、startKobuki.sh 三个脚本文件中

最后只需要执行命令 ./startKobuki | python getLocationLocal.py 命令就可以启动获取机器人位置程序，并实时的将位置信息保存到数据库中。

获取结果如下：