ros에서 Kinect의'방귀벌레'를 내려주세요.

ros에서 Kinect의'방귀벌레'를 내려주세요.
이 글은 잊혀지지 않도록 오래 전 업무의 정리를 위해 기록으로 삼았다.순전히 필기다.
구현된 기능: Kinect로 사람을 따라간다.
플랫폼: Ubuntu 14.04+Kinect1+전방향 모바일 플랫폼
1. Kinect 드라이브 설치
인터넷상의 설치 방법을 참고할 수 있으며, 이미 상세한 절차가 있으니, 더 이상 군말하지 않겠다.Kinect1이 설치되어 있어 정상적으로 사용할 수 있습니다.
2. OpenNi2+NITE 설치
뼈대 추출을 위한 라이브러리입니다.
3. 오픈니 설치tracker
4. tf변환을 받아 로봇 속도 제어 명령을 발포한다.
주로lookupTransform 함수로user1의 좌표계를 받아서 좌표계의 좌표를 얻어 거리에 따라 로봇이 어떤 속도 명령을 내릴지 판단한다.
문제는 다음과 같습니다.
(1)user1을 잃어버리면 로봇이 참조 기준을 잃고 날아간다.
(2) 주변에 사람이 많을 때 로봇은 혼란스럽고 추적 효과가 따라가지 못하기 때문에 배경의 영향이 매우 크고 보통 한 사람만 하는 상황에서 진행한다.
(3) 로봇 속도의 영향은 현재 모두 비교적 느린 것으로 보통 0.3m/s 정도이고 너무 빠르면 로봇이 반응하지 못한다.
(4)    openni_tracker 자체의 코드도 일반적으로 쓰여져서 너무 높은 노봉성을 가지지 못하고 때때로 자주 붕괴된다.
이런 상황들은 인터넷에서 많은 사람들의 문제를 본 후에 모두가 만나게 될 것이다.그러나 좋은 해결 방법은 없다.뼈대 식별이 좋은 버전의 오픈니와 오픈니 로 바꾸자는 제안이 나왔다tracker, 그러나 이런 방법은 내가 시도하지 않았다.
문제 (1)에 대해 내가 시도한 개선은 두 가지를 포함한다.첫째, 원래의 수신 tf변환 코드를 다시 작성하여 이상이 있는 상황을 판단하여 로봇을 멈추게 하고 날아다니는 것을 피한다.둘째는 오픈니2트랙터 중에 트랙터.cpp 코드, 이상 판단 조건과 처리를 추가합니다.
문제(2)에 대해 첫 번째 생각은 색깔이나 라벨 등을 넣는 것이다. 나중에 다른 사람의 블로그를 보면 직접 얼굴인식으로 이렇게 하는 것이 있는데 로봇은 보통 한 사람만 추적할 수 있다. 여러 사람이 있을 때 어떻게 이 사람을 판단할 수 있는지 같은 문제에 부딪힐 수 있다.많은 사람들이 미행하는 것에 관해서는 정말 좋은 아이디어가 없다.
개인적인 경험은 계속 생각해야 합니다. 좋은 생각이 있으면 메일로 보내주세요[email protected], 많이 교류하세요.
첨부 코드: Kinecttf.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<:twist> ("/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;
}


 
   
 

좋은 웹페이지 즐겨찾기