ros下Kinect的“跟屁虫”

ros下Kinect的“跟屁虫”

此文为很久之前工作的整理,以此作为记录,以免遗忘。纯属笔记。

实现的功能:用Kinect跟随人。

平台:Ubuntu14.04+Kinect1+全向移动平台

1.     安装Kinect驱动

可以参考网上的安装方法,已经有详细的步骤了,不再赘述。Kinect1已经安装好了,可以正常使用。

2.     安装OpenNI2+NITE

进行骨架提取的库。

3.     安装openni_tracker

4.     接听tf变换,发布机器人速度控制命令。

主要是用lookupTransform函数来接听user1的坐标系,从而获得坐标系的坐标,根据距离判断机器人发出何种速度命令。

存在的问题有:

(1)    user1丢失时,机器人失去参照标准,会跑飞;

(2)    当周围的人多时,机器人将会混乱,跟踪效果往往会跟丢,所以背景的影响很大,一般只在一个人的情况下进行;

(3)    机器人速度影响,目前都是比较慢的,一般在0.3m/s左右,太快机器人反应不过来;

(4)    openni_tracker本身的代码写的也一般,不具有太高的鲁棒性,有时经常崩溃。

这些情况,在网上看了很多人的问题后,大家都会遇到。然而好的解决方法没有。有人提议是,换成对骨架识别比较好的版本的openni及openni_tracker,然而这种方法我没有尝试。

针对问题(1),我所尝试做的改进包括两个方面。一是将原来的接听tf变换的代码重新写,对有异常的情况作出判断,使机器人停止,从而避免跑飞;二是改变openni2_tracker中tracker.cpp代码,加入异常判断条件和处理。

针对问题(2),初次的想法是加入颜色或者标签等,后来看别人的博客中有直接用人脸识别这样来做,但是机器人一般只能跟踪一个人,当有多个人时怎么能判断出来这个人,所以应该也会遇到同样的问题。至于多人跟踪,实在是没有好的idea。

个人经验,仍需继续思考,如有好的想法,欢迎给我发邮件[email protected],多多交流。

附代码:Kinect_tf.cpp

/*************************************************************************************
*Name:kinect_tf.cpp
*Date:2017-02-28
*Author:Zhanghuijuan
*Function:follow people instantly.
*************************************************************************************/
//头文件
#include 
#include 
#include 
#include //播放音频
#include 
#include "std_msgs/String.h"

const char *str_distance,*str_angle;
int count_running = 0;
/*主函数*/
int main (int argc, char** argv)
{
	ros::init (argc, argv, "kinect_tf ");//发布的topic
	ros::NodeHandle nh;
	sound_play::SoundClient sc;	
	geometry_msgs::Twist cmd;
	ros::Publisher pub = nh.advertise ("/cmd_extvel", 10);//发布消息到 topic /cmd_extvel
	tf::TransformListener listener;
	ros::Rate rate(10.0);
	while (node.ok()){
		//接听消息
		tf::StampedTransform tf, tf_right_hand, tf_right_shoulder; //tf变换的变量
		try{		
			listener.lookupTransform("/openni_depth_frame", "/torso_1", ros::Time(0), tf);  //接听话题/torso_1消息,我们查询listener为一个指定的转换,有四个主要参数:
			//1,2我们想要从1转换到2,3转换的时间,4用transform来存储转换结果
			double pose_x = tf.getOrigin().x();
			double pose_y = tf.getOrigin().y();
			double pose_theta = tf::getYaw(tf.getRotation());
			//ROS_INFO("Pose of User: [x,y,theta]: [%f, %f, %f]...", x, y, theta);
			listener.lookupTransform("/openni_depth_frame", "/right_hand_1", ros::Time(0), tf_right_hand);//接听right_hand_1消息
			double tf_right_hand_z = tf_right_hand.getOrigin().z();
			listener.lookupTransform("/openni_depth_frame", "/right_shoulder_1", ros::Time(0), tf_right_shoulder);//接听right_shoulder_1消息
			double tf_right_shoulder_z = tf_right_shoulder.getOrigin().z();
		}
		catch (tf::TransformException& ex){
			ROS_ERROR("Received an exception trying to get information: %s", ex.what());//抛出异常
			ros::Duration(1.0).sleep();
			continue;
		}
		/*step1:根据距离信息判断,从而发送控制速度指令,针对全向平台*/
		cmd.linear.x = 0.0;//初始速度x方向为0
		cmd.linear.y = 0.0;
		cmd.angular.z = 0.0;
		//X方向
		if (pose_x < 1.6)	//距离远近,行为定义
		{
			str_distance = "knee thai jean ler";//近了
			cmd.linear.y = 0.25;
		}
		else if (pose_x > 2.0)
		{
			str_distance = "knee thai yuan ler";//远了
			cmd.linear.y = -0.25;
		}
		else//距离正好
		{
			str_distance = "hen how";
			cmd.linear.y = 0.0;
		}
		//Y方向
		if (pose_y > 0.05)
		{
			str_angle = "zai Geo bian";//偏左
			cmd.angular.z = 0.15;
		}
		else if (pose_y < -0.05)
		{
			str_angle = "zai you bian";//偏右
			cmd.angular.z = -0.15;
		}
		else//正好
		{
			str_angle = "zai june jain";
			cmd.angular.z = 0.0;
		}
		/*step2:手势指令*/
		if (tf_right_hand_z >= tf_right_shoulder_z)//手势停止:当右手高于右肩膀时,机器人停止
		{
			cmd.linear.x = 0.0;
			cmd.linear.y = 0.0;
			cmd.angular.z = 0.0;
			pub.publish(cmd);
			ROS_INFO("cmd published: [v_x,v_y,w]:[%f, %f, %f]", cmd.linear.x, cmd.linear.y, cmd.angular.z);
			//播放声音
			const char *str1 = "I am stopping now!";
			sc.repeat(str1);
			sleep(4);
			sc.stopSaying(str1);
			loop.sleep();
			continue;
		}
		//发布速度
		pub.publish(cmd);
		ROS_INFO("cmd published: [v_x,v_y,w]:[%f, %f, %f]", cmd.linear.x, cmd.linear.y, cmd.angular.z);
		count_running++;
		if (count_running > 30)
		{
			const char *str2 = "/home/cnimi/Downloads/wavetest/apert.wav";//调用音频文件
			sc.startWave(str2);
			//sleep(1);
			//sc.stopWave(str2);
			count_running = 0;
		}
	}			
	return 0;
}


 
  

你可能感兴趣的:(ROS实现,ROS,kinect,跟随)