/×
kinect_tf.cpp
实现人体骨骼检测追踪到的坐标点用tf转换接听,得到关节点的信息,这里只是接听了部分人体位置数据,
“right_hand_1,right_shoulder_1”
并对接听到的数据进行处理,这里判断距离,如果距离过大要调整速度,距离过小则调整速度,手部关节点如果y方向坐标如果高于肩部,则停下来了
×/
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <sound_play/sound_play.h>//播放音频
#include <unistd.h>
#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<geometry_msgs::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;
}
机器人视觉项目:视觉检测识别+机器人跟随(8)
猜你喜欢
转载自blog.csdn.net/Synioe/article/details/82793340
今日推荐
周排行