A-A+

手势控制turtlebot机器人状态的软件实现

2012年12月08日 ROS 评论 2 条 阅读 48,170 次
摘要:

通过控制者不同的手势控制turtlebot机器人在跟随、键盘远程控制和停止三种状态下的切换。

1.实现目标
通过控制者不同的手势控制turtlebot机器人在跟随、键盘远程控制和停止三种状态下的切换。

2.基本思路

Turtlebot使用开源的ROS(Robot Operating System )作为操作系统。ROS众多的开源程序包(package)中已经提供了机器人跟随(turtlebot_follower程序包)和遥控(turtlebot_teleop程序包)功能,且pi_tracker程序包在OpenNI的支持下利用Kinect可以捕获人体各关节的位置信息(openni_tracker也能实现类似功能)。通过各关节的位置信息就能计算识别出特定的人体手势。而根据识别出的手势执行特定的动作至少有两种途径:一是重新编写整合手势识别、跟随(follower)和遥控(teleop)功能的新程序包;一是编写根据手势判别结果通过子进程的方式动态地运行和终止follower和teleop功能的守护控制程序。相比较而言,第二种方式比较易于实现。

3.具体实现

目前ROS的程序包(packages)主要用C++和Python两种语言编写,考虑到调试方便性和开发便捷性,本实现采用Python语言开发,将守护控制程序做成package,最后构造启动所有节点(nodes)的launch文件(launch文件通过roslaunch可单命令运行多个nodes)。

3.1手势的识别

Pi_tracker package中的tracker_skeleton node发布包含各关节位置信息的Skeleton主题(topic),守护控制程序通过订阅此topic获得所需的人体关节信息,位置信息用三维坐标表示,坐标轴定义如图1:

三维坐标系定义

图1 三维坐标系定义(来源于网络)

此topic的消息模型如下:

Header header   //包含图像序列号,时间戳
int32 user_id  //捕获到的人体编号
string[] name  //<em>人体关节名称
float32[]confidence  //</em><em>值的可信度
geometry_msgs/Vector3[] position  //关节位置坐标值
geometry_msgs/Quaternion[] orientation  //方向信息</em>

由于靠图像识别得出的位置值不可能很精确,手势判别的具体算法要用比较固定的相对位置值与变化的相对位置值作比较。这里我们选肩宽作基准值,若右手举起,右手与左手的Y坐标的差值应大于肩的宽度,而对于两手平举还说,两手和其同侧的肘部的Y坐标的差值应很小,一般来说都应小于1/3个肩宽。

3.2动作的实现

按照前面的思路,要创建子进程来加载动作node,而且子进程要可控,在恰当的时候要终止其运行。对于teleop来说,还需注意一个问题,就是子进程的输入设备必须是当前的shell的标准输入,否则teleop就不能捕获按键,完成不了遥控的动作。考虑到这些因素,子进程的创建用subprocess模块的popen()构造函数实现,属于popen类,其原型如下:

class subprocess.Popen(args, bufsize=0, executable=None, stdin=None, stdout=None, stderr=None, preexec_fn=None, close_fds=False, shell=False, cwd=None, env=None, universal_newlines=False, startupinfo=None, creationflags=0)

这里我们只需设置args和shell两个参数。args参数设置运行命令及参数,shell参数指定命令是否在shell中执行。为了能使teleop获取按键输入,shell参数必须设置为False。

由于从launch文件运行node的工具roslaunch也是通过Popen完成节点的启动的,即守护控制程序创建子进程运行roslaunch,roslaunch又创建了自己的子进程运行node,所以在终止子进程时用terminate()方法,向所有子进程发送SIGTEM信号,确保实现动作的node真正退出。

守护控制程序的流程如图2

守护控制程序流程
图2 程序流程

主要代码(修改自tracker_command.py):

#!/usr/bin/env python
#coding=gbk

import roslib; roslib.load_manifest('pi_tracker')
import rospy
import pi_tracker_lib as PTL
from pi_tracker.msg import Skeleton
from pi_tracker.srv import *
import PyKDL as KDL
import time
import subprocess

class TrackerCommand():
# __init__函数为python语言中类的初始化函数,在类对象创建时执行
# 在这个函数中主要进行了节点的初始化,设置获取topic消息的频率,订阅Skeleton,创建存储人体关节信息的字典型变量skeleton
# 定义了字典型手势查询表gestures,用于后面查找手势对应的判别函数

    def __init__(self):
        rospy.init_node('tracker_command')  #初始化节点
        rospy.on_shutdown(self.shutdown)    #注册节点关闭时执行的函数shutdown()

        # 设置分发或订阅、命令等的频率      
        self.rate = rospy.get_param("~command_rate", 1)
        rate = rospy.Rate(self.rate)

        # 订阅skeleton topic,skeleton_handler()为消息处理函数
        rospy.Subscriber('skeleton', Skeleton, self.skeleton_handler)

        # 定义字典skeleton,存储获取的消息数据,包括可信度值,位置坐标,方向坐标
        self.skeleton = dict()
        self.skeleton['confidence'] = dict()
        self.skeleton['position'] = dict()
        self.skeleton['orientation'] = dict()        

        # 定义了字典型手势查询表gestures,用于后面查找手势对应的判别函数

        self.gestures = {'right_hand_up': self.right_hand_up,
                         'left_hand_up': self.left_hand_up,
                         'arms_up': self.arms_up
                         }
        self.bot_follow = False
        self.bot_stop = True
        self.b_process = False
        self.t_process = ''
        #self.count = 1
        # 初始化命令标记为stop
        self.tracker_command = "STOP"

        rospy.loginfo("Initializing Tracker Command Node...") #记录日志信息

        # 此循环主功能实现区域,程序运行后到结束都在此循环内
        while not rospy.is_shutdown():            
            self.shoulder_width = PTL.get_body_dimension(self.skeleton, 'left_shoulder', 'right_shoulder', 0.4) # 获取两肩的相对距离值  
            self.tracker_command = self.get_command(self.tracker_command) # 进行手势判别并执行动作,见get_command()函数     
            rate.sleep() #根据频率设置,等待下一次动作时机

    # 右手的判别算法       
    def right_hand_up(self):
        if self.confident(['right_hand', 'left_hand']):          
            if (self.skeleton['position']['right_hand'].y() - self.skeleton['position']['left_hand'].y()) / self.shoulder_width &gt; 1:
                return True
        return False

    #左手的判别算法      
    def left_hand_up(self):
        if self.confident(['right_hand', 'left_hand']):            
            if (self.skeleton['position']['left_hand'].y() - self.skeleton['position']['right_hand'].y()) / self.shoulder_width &gt; 1:
                return True
        return False

    #双手平举的算法,只判断是平举,前举或侧举都可       
    def arms_up(self):
        if self.confident(['left_elbow', 'right_elbow', 'left_hand', 'right_hand']):           
            if (abs(self.skeleton['position']['left_elbow'].y() - self.skeleton['position']['left_hand'].y()) &lt; self.shoulder_width/3) and \
                 (abs(self.skeleton['position']['right_elbow'].y() - self.skeleton['position']['right_hand'].y()) &lt; self.shoulder_width/3):
                return True
        return False

    # 此函数查看人体关节位置值是否可信,用于手势判别中,参与运算的任一关节的任一值可信度值低于0.5,手势判别则失败
    def confident(self, joints):
        try:
            for joint in joints:
                if self.skeleton['confidence'][joint] &lt; 0.5:                    
                    return False
            return True
        except:
            return False

    #进行手势判别并执行相应动作
    def get_command(self, current_command): 
        try:
            command = current_command

            # 举起右手执行follower
            if self.gestures['right_hand_up'](): # 通过字典查找手势对应的判别函数,此处是执行right_hand_up()函数
            #if self.count = 30 and self.count = 60:
                if self.bot_stop == False:
                    command = "STOP"
                    self.bot_follow = False
                    try:                        
                        if self.b_process:
                            self.t_process.terminate()
                            self.b_process = False              
                    except:
                        pass
                else:
                    command = current_command   

            else:
                command = current_command;

            if command != current_command:
                rospy.loginfo("Gesture Command: " + command)    #手势改变并被识别后进行日志记录

            return command
        except:
            return current_command

     # 订阅消息处理函数   
    def skeleton_handler(self, msg):
        for joint in msg.name:  
            self.skeleton['confidence'][joint] = msg.confidence[msg.name.index(joint)]
            self.skeleton['position'][joint] = KDL.Vector(msg.position[msg.name.index(joint)].x, msg.position[msg.name.index(joint)].y, msg.position[msg.name.index(joint)].z)
            self.skeleton['orientation'][joint] = msg.orientation[msg.name.index(joint)]

    #节点关闭时执行函数
    def shutdown(self):
        rospy.loginfo("Shutting down Tracker Command Node.")

if __name__ == '__main__':  #类似于C和C++中的main()函数,只在本程序自己执行时运行,外部程序import时不执行
    try:
        TrackerCommand() #构造TrackerCommand类对象
    except rospy.ROSInterruptException:
        pass

PS:代码看不全的请复制下来看,注意换行,代码高亮插件自动换行了。

2 条留言  访客:2 条  博主:0 条

  1. myhoney

    :roll:

  2. 国外新鲜资讯

    来看看,串门来了,博客不错,可以和博主文章互评吗,就是帮对方最新文章评论一次!让网站有点人气!对大家有好处,谢谢!互评加我QQ1005560448 注明站名。本人已在博客大全做了长期广告!

给我留言

Copyright © ExBot易科机器人实验室 保留所有权利.   Theme   Robin modified by poyoten

用户登录

分享到: