基于DragonBoard 410c的家庭智能环保卫士——（6）kobuki位置信息传输及显示

     前一个blog中教大家如何利用kobuki驱动中提供的demo程序实时的获取kobuki机器人的位置信息并保存到数据库中，这里进一步教大家如何将保存在数据库中的实时的位置信息传回到另一个用DragonBaord 410C做的上位机上，并且将位置信息和路径信息实时的显示出来，为后续进一步做路径规划功能提供数据基础。

     这里我们的基本思路是，控制kobuki机器人的Dragonboard 410c 开发板负责采集机器人位置信息，运行http://blog.csdn.net/andymfc/article/details/62422472中的程序，同时我们这里对程序进行简单的修改，不将数据保存到数据库中，而是直接上传到，在另外一个Dragonbaord410c上搭建的TCP服务器上（服务器的搭建和编程通信在http://blog.csdn.net/andymfc/article/details/55258011中有详细介绍），服务器接收位置数据然后进行解析，将数据实时的保存到sqlite数据库中，并且实时更新数据，最后我们只需要从这些数据库中取出数据，然后实时的显示机器人位置信息即可，这里我们使用了turtle绘图工具来进行绘图显示，这样我们就可以实时的动态开到机器人的位置信息和移动路径。

    首先我们需要在服务器端构建服务器程序，这里，我们依然利用了http://blog.csdn.net/andymfc/article/details/62422472中的locationDB.py文件来操作数据库保存位置信息到服务器的sqlite3数据库，同时编写下面代码实现服务器：

 import sys
from gevent.server import StreamServer
from keyControl import virtKeyControl
from locationDB import locationDB
BUFSIZE=1024
def handle(socket,address):
    flag=0
    currentLocation=''
    while(1):
        try:
            print("start get location by kobuki robot")
            while 1:
                locReport=socket.recv(BUFSIZE)
                print(locReport)
                if locReport=="current pose: [0, 0, 0]\n":
                    flag=1
                if flag==1: 
                    currentLocation=(locReport[15:-2]).split(', ',2)
                    print(currentLocation[0])
                    print(currentLocation[1])
                    print(currentLocation[2])
                    currentX=float(currentLocation[0])
                    currentY=float(currentLocation[1])
                    currentA=float(currentLocation[2])
                    if currentA>0:
                        currentA=currentA*180/3.2
                    else:
                        currentA=360+currentA*180/3.2
                    print(currentLocation)
                    print(type(currentLocation))
                    LDB=locationDB("./locationDB.db")
                    LDB.updateCurrentLocation(currentX,currentY,currentA)
        except:
            print("kobuki control service error\n")
            #socket.send("kobuki control service error\n")
if __name__=="__main__":
    kobukiControl=StreamServer(('192.168.43.131',5000),handle)
    kobukiControl.serve_forever()

    然后是对http://blog.csdn.net/andymfc/article/details/62422472中的程序进行简单修改，事先向服务器报告位置数据，具体代码如下：

###在import代码块区域，增加如下代码

from socket import AF_INET,SOCK_STREAM,socket

同时增加全局变量
HOST='192.168.43.156'
PORT=5000
BUFSIZE=512
ADDR=(HOST,PORT)

在getRobotLocation类的初始化函数中增加

self.client=socket

同时修改初始化函数，增加一个参数具体如下：

def __init__(self,socket):

修改def startGetThread(self):函数注释掉原来数据库代码，在后面增加服务器发送消息代码

#LDB=locationDB("./locationDB.db")
#LDB.updateCurrentLocation(self.currentX,self.currentY,self.currentA)
self.client.send(self.locReport)

最后修改主函数如下：

client=socket(AF_INET,SOCK_STREAM)
client.connect(ADDR)
robotLocation=getRobotLocation(client)
location=robotLocation.startGetThread()

按照上述修改，然后按照http://blog.csdn.net/andymfc/article/details/62422472方法启动程序就可以连接服务器发送和上报数据了。

这里我们就完成了kobuki位置信息的远程传输交互设计，只需要先在服务器端运行服务器脚本，然后启动客户端程序即可将kobuki机器人的位置信息通过网络实时的报告到服务器。

 最后我们就可以在服务器端编写一个图形化的显示程序，实时的显示kobuki机器人的位置信息和移动轨迹了，这里我们使用了turtle 绘图工具来实现，详细的操作方法大家可以参考turtle官网，这里我们只是从数据库中取出数据，然后进行绘图显示，具体的代码如下：

   class locationView:
    def __init__(self,x,y,a):
        self.currentX=x
        self.currentY=y
        self.currentA=a
        self.historyX=0
        self.historyY=0
        self.historyA=0
        self.distA=0
        self.distL=0.0
        self.t=turtle.Pen()
    def updateLocation(self):
        #self.t.goto(0,0)
        self.t.speed(2)
        self.t.down()
        self.t.color("red")
        self.t.pensize(4)
        self.distL=math.sqrt((self.currentX-self.historyX)**2+(self.currentY-self.historyY)**2)
        if self.distL!=0:
            if self.currentY-self.historyY>0:
                self.distA=math.acos((self.currentX-self.historyX)/self.distL)*180/3.1415926
            else:
                self.distA=180+math.acos((self.currentX-self.historyX)/self.distL)*180/3.1415296
                print(self.distA)
            self.t.right(self.distA-self.historyA)  
            self.t.forward(self.distL*100)
            self.t.right(self.currentA-self.distA)
        else:
            self.t.right(self.currentA-self.historyA)
            #self.t.right(self.currentA-self.distA)
        self.historyA=self.currentA
        self.historyX=self.currentX
        self.historyY=self.currentY
        self.historyA=self.currentA                  
    def reset(self):
        self.t.reset()
    def drawGrid(self,size,m,n):
        self.t.down()
        self.t.reset()
        self.t.speed(10)
        self.t.left(90)
        for i in range(0-m,m+1):
            if i%size==0:
                #self.t.reset()
                self.t.up()
                self.t.goto(i,0-n)
                self.t.down()
                self.t.forward(n*2)
        self.t.right(90)
        for i in range(0-n,n+1):
            if i%size==0:
                self.t.up()
                self.t.goto(0-m,i)
                self.t.down()
                self.t.forward(m*2)
        self.t.up()
        self.t.goto(0,0)
    def updateLocation2(self):
        self.t.goto(self.currentX*50,self.currentY*50)
        self.t.right(self.historyA)
        #self.t.left(self.currentA)       
    def updateLocationByDB(self):
        print("updateLocationThread")
        LDB=locationDB("./locationDB.db")
        [x,y,a]=LDB.getCurrentLocation()
        print(x)
        print(y)
        print(a)
        self.currentX=x
        self.currentY=y
        self.currentA=a
        self.updateLocation()
    def testFunction(self):
        self.t.setheading(90)
if __name__ == '__main__':
    view=locationView(0,0,0)
    view.drawGrid(20,200,200)
    while 1:
        view.updateLocationByDB()
        time.sleep(1)

    保存上述代码到locationView.py中，然后在启动服务器程序和koubuki机器人后运行即可，效果如下：