一、前言
NAO机器人接触有一年多了，期间开发了很多的程序。
—最开始是做NAO机器人人脸识别和室内复杂环境下的路径规划；
—后来参加NAO机器人高尔夫球赛，开发了一套完整的NAO 机器人打高尔夫球赛的框架，并将源码提供给了举办方。
—在此基础之上，又帮助举办方开发了一套NAO机器人接力赛程序框架。第一个版本是机器人通过Socket通信实现机器人消息传输，也是本章节提及的；第二个版本为机器人通过机器视觉来提取信息。
下面就来说说socket通信吧。
TCP服务端：
1 创建套接字，绑定套接字到本地IP与端口
# socket.socket(socket.AF_INET,socket.SOCK_STREAM) , s.bind()
2 开始监听连接 #s.listen()
3 进入循环，不断接受客户端的连接请求 #s.accept()
4 然后接收传来的数据，并发送给对方数据 #s.recv() , s.sendall()
5 传输完毕后，关闭套接字 #s.close()
TCP客户端:
1 创建套接字，连接远端地址
# socket.socket(socket.AF_INET,socket.SOCK_STREAM) , s.connect()
2 连接后发送数据和接收数据 # s.sendall(), s.recv()
3 传输完毕后，关闭套接字 #s.close()
对于NAO机器人，无非就是一个作为服务器，一个作为客户端，代码如下：
client.py
#-*- coding: utf-8 -*-
###########################################################
# > Description: 远程控制-客户端
# 向服务器发送指令，查看服务器回执消息。
###########################################################
#! /usr/bin/env python
import argparse
from naoqi import ALProxy
import socket
import time
LISTEN_PORT = 8001 # 服务器监听端口
# command
COMMAND_DISCONNECT = 'DISCONNECT'
COMMAND_HEADYAW = 'HEADYAW' # 头左右
COMMAND_HEADPITCH = 'HEADPITCH' # 头上下
# flag
CONNECT = False
def main(robot_IP, robot_PORT=9559):
# ----------> 连接socket服务器监听端口 <----------
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((robot_IP, LISTEN_PORT))
time.sleep(2)
CONNECT = True
while CONNECT == True:
# 输入指令
command = raw_input("Command code:")
# socket 发送指令
sock.send(command)
if command == COMMAND_HEADYAW or command == COMMAND_HEADPITCH:
value = raw_input("Value:")
sock.send(value)
# socket 接受返回消息
buf = sock.recv(1024)
print buf
if command == COMMAND_DISCONNECT:
CONNECT = False
sock.close() # 与服务器端断开socket连接
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="192.168.1.100", help="Robot ip address")
parser.add_argument("--port", type=int, default=9559, help="Robot port number")
args = parser.parse_args()
# ----------> 执行main函数 <----------
main(args.ip, args.port)
server.py
#! /usr/bin/env python
#-*- coding: utf-8 -*-
###########################################################
# > Description: 远程控制-服务器端
# 接受客户端发来的指令，执行相应功能。
###########################################################
import argparse
from naoqi import ALProxy
import socket
import sys # sys.exit() 退出main函数
import almath # 角度转弧度(almath.TO_RAD)
import thread # 多线程
import time # 延时函数 time.sleep(1)
LISTEN_PORT = 8001 # 服务器监听端口
# Command 定义
COMMAND_WAKEUP = 'WAKEUP'
COMMAND_REST = 'REST'
COMMAND_FORWARD = 'FORWARD'
COMMAND_BACK = 'BACK'
COMMAND_LEFT = 'LEFT'
COMMAND_RIGHT = 'RIGHT'
COMMAND_STOP = 'STOP'
COMMAND_TURNLEFT = 'TURNLEFT'
COMMAND_TURNRIGHT = 'TURNRIGHT'
COMMAND_DISCONNECT = 'DISCONNECT'
COMMAND_SENSOR = 'SENSOR'
COMMAND_HEADYAW = 'HEADYAW' # 头左右
COMMAND_HEADPITCH = 'HEADPITCH' # 头上下
# flag
CONNECT = False # 客户端连接Flag
SENSOR = False # 传感器监控Flag, 为True则有线程定时发送数据；
# 全局变量，供其他函数使用
ROBOT_IP = '192.168.1.100'
ROBOT_PORT = 9559
connection = None
battery = None
memory = None
def main(robot_IP, robot_PORT=9559):
global ROBOT_IP
global ROBOT_PORT
ROBOT_IP = robot_IP
ROBOT_PORT = robot_PORT
# ----------> Connect to robot <----------
tts = ALProxy("ALTextToSpeech", robot_IP, robot_PORT)
motion = ALProxy("ALMotion", robot_IP, robot_PORT)
posture = ALProxy("ALRobotPosture", robot_IP, robot_PORT)
global memory
memory = ALProxy("ALMemory", robot_IP, robot_PORT)
leds = ALProxy("ALLeds", robot_IP, robot_PORT)
global battery
battery = ALProxy("ALBattery", ROBOT_IP, ROBOT_PORT)
autonomous = ALProxy("ALAutonomousLife", robot_IP, robot_PORT)
autonomous.setState("disabled") # turn ALAutonomousLife off
# ----------> 开启socket服务器监听端口 <----------
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.bind((robot_IP, LISTEN_PORT))
sock.listen(10)
try:
while True: # 等待客户端连接，单线程监听单一客户端
global connection
connection,address = sock.accept()
CONNECT = True
while CONNECT == True:
try:
#connection.settimeout(10)
# 服务器接受指令
buf = connection.recv(1024)
print "get:[", buf, "]"
# 根据接受的命令执行不同操作
if buf == COMMAND_WAKEUP:
if motion.robotIsWakeUp() == False:
motion.post.wakeUp()
connection.send("Robot Motion: [ Wakeup ]\r")
elif buf == COMMAND_REST:
if motion.robotIsWakeUp() == True:
motion.post.rest()
connection.send("Robot Motion: [ Rest ]\r")
elif buf == COMMAND_FORWARD:
if motion.robotIsWakeUp() == False:
motion.post.wakeUp()
connection.send("Robot Motion: [ Wakeup ]\r")
motion.post.moveInit()
connection.send("Robot Motion: [ MoveInit ]\r")
#motion.post.moveTo(0.3, 0, 0)
motion.move(0.1,0,0) # 固定速率持续行走
connection.send("Robot Motion: [ Forward ]\r")
elif buf == COMMAND_BACK:
if motion.robotIsWakeUp() == False:
motion.post.wakeUp()
connection.send("Robot Motion: [ Wakeup ]\r")
motion.post.moveInit()
connection.send("Robot Motion: [ MoveInit ]\r")
#motion.post.moveTo(-0.1, 0, 0)
motion.move(-0.1,0,0)
connection.send("Robot Motion: [ Back ]\r")
elif buf == COMMAND_LEFT:
if motion.robotIsWakeUp() == False:
motion.post.wakeUp()
connection.send("Robot Motion: [ Wakeup ]\r")
motion.post.moveInit()
connection.send("Robot Motion: [ MoveInit ]\r")
#motion.post.moveTo(0, 0.1, 0)
motion.move(0,0.1,0)
connection.send("Robot Motion: [ Left ]\r")
elif buf == COMMAND_RIGHT:
if motion.robotIsWakeUp() == False:
motion.post.wakeUp()
connection.send("Robot Motion: [ Wakeup ]\r")
motion.post.moveInit()
connection.send("Robot Motion: [ MoveInit ]\r")
#motion.post.moveTo(0, -0.1, 0)
motion.move(0,-0.1,0)
connection.send("Robot Motion: [ Right ]\r")
elif buf == COMMAND_STOP:
motion.stopMove()
connection.send("Robot Motion: [ stop move ]\r")
elif buf == COMMAND_TURNRIGHT:
if motion.robotIsWakeUp() == False:
motion.post.wakeUp()
connection.send("Robot Motion: [ Wakeup ]\r")
motion.post.moveInit()
connection.send("Robot Motion: [ MoveInit ]\r")
motion.move(0, 0, -0.3)
connection.send("Robot Motion: [ turn right ]\r")
elif buf == COMMAND_TURNLEFT:
if motion.robotIsWakeUp() == False:
motion.post.wakeUp()
connection.send("Robot Motion: [ Wakeup ]\r")
motion.post.moveInit()
connection.send("Robot Motion: [ MoveInit ]\r")
motion.move(0, 0, 0.3)
connection.send("Robot Motion: [ turn left ]\r")
elif buf == COMMAND_DISCONNECT:
CONNECT = False
connection.send("disconnect from robot server.\r")
elif buf == COMMAND_HEADYAW:
# 头部左右转动(Yaw轴)
buf2 = connection.recv(1024) # 读取Yaw值
print "yaw:", buf2
angles = (int(buf2) - 50)
motion.setStiffnesses("Head", 1.0)
motion.setAngles("HeadYaw", angles * almath.TO_RAD, 0.2) # 以10%的速度转换angles角度
connection.send("Robot Motion: [ head raw ]\r")
elif buf == COMMAND_HEADPITCH:
# 头部上下转动(Pitch轴)
buf2 = connection.recv(1024) # 读取Pitch值
print "pitch:", buf2
angles = (int(buf2) - 50) * 2
motion.setStiffnesses("Head", 1.0)
motion.setAngles("HeadPitch", angles * almath.TO_RAD, 0.2) # 以10%的速度转换angles角度
connection.send("Robot Motion: [ head pitch ]\r")
elif buf == COMMAND_SENSOR:
global SENSOR
if SENSOR == False:
# 开启新线程，定时发送传感器数据
SENSOR = True
thread.start_new_thread(sensor, (1,)) # 2nd arg must be a tuple
else:
# 第二次发送COMMAND_SENSOR, 则关闭线程
SENSOR = False # 设置标识位，线程检测后自己退出。
connection.send("Robot Motion: [ send sensor value ]\r")
else:
connection.send(buf + ": command not found\r")
except socket.timeout:
print 'time out'
connection.close() # 关闭当前socket连接，进入下一轮循环
except KeyboardInterrupt: # CTRL+C, 关闭服务器端程序;
print ""
print "Interrupted by user, shutting down"
sys.exit(0)
def sensor(interval):
''' 每interval秒，发送一次传感器数据
'''
while SENSOR == True:
connection.send("BATTERY" + "#" + str(battery.getBatteryCharge()) + "\r")
connection.send("SONAR1" + "#" + str(memory.getData("Device/SubDeviceList/US/Left/Sensor/Value")) + "\r")
connection.send("SONAR2" + "#" + str(memory.getData("Device/SubDeviceList/US/Right/Sensor/Value")) + "\r")
# print "BATTERY" + "#" + str(battery.getBatteryCharge())
time.sleep(interval)
# SENSOR == False
thread.exit_thread()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--ip", type=str, default="192.168.1.100", help="Robot ip address")
parser.add_argument("--port", type=int, default=9559, help="Robot port number")
args = parser.parse_args()
# ----------> 执行main函数 <----------
main(args.ip, args.port)
第一台机器人到达终点立即给第二台机器人发一个指令即可。接力跑还是很容易啊。重点在‘’跑‘’！