Baxter实战:Ubuntu16.04+Kinect2实现动作跟随
Kinect V2**
1.下载libfreenect2资源:
git clone https://github.com/OpenKinect/libfreenect2.git
cd libfreenect2
2.下载更新包:
cd depends
./download_debs_trusty.sh
3.安装工具:
sudo apt-get install build-essential cmake pkg-config
4.安装libusb:
sudo dpkg -i debs/libusb*deb
5.安装TurboJPEG
sudo apt-get install libturbojpeg libjpeg-turbo8-dev
6.安装OpenGL
sudo dpkg -i debs/libglfw3*deb
sudo apt-get install -f
7.安装OpenCL(可选)-GPU加速:
这里不装,如若需要装,则参见相关的文档,可用下面的代码进行安装,应该可以
sudo apt-get install opencl-headers
8.安装OpenNI2
kinect是微软的设备,故专门有kinect.dll,而linux上只有专家开发出来的适配驱动。目前在网上的资料看,OpenNi兼容不了v2,而v2驱动都是由OpenNi2开发的,没有用OpenNi开发的。故对于v1设备,只需要安装OpenNi就可以了,而对于v2设备,则需要安装OpenNi2。
sudo apt-add-repository ppa:deb-rob/ros-trusty
sudo apt-get update
sudo apt-get install libopenni2-dev
9.编译 libfreenect2
cd ~/libfreenect2
mkdir build && cd build
cmake .. -DENABLE_CXX11=ON
make
make install
10.获取设备权限
sudo cp platform/linux/udev/90-kinect2.rules /etc/udev/rules.d/
连接好kinect2的设备,以免报错说找不到设备
11.测试(进入build目录下):
./bin/Protonect pl //测试OpenGL是否正常
./bin/Protonect cl //测试opencl是否正常
./bin/Protonect cpu //测试CPU是否正常
如若正常,则可以看到相应的View图
12.ROS的安装
在安装前需注意ROS与Ubuntu的对应配置
所以,这里选择ROS Kinectic Kame进行安装
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
sudo apt-get update
sudo apt-get install ros-kinetic-desktop-full
sudo rosdep init
rosdep update
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt-get install python-rosinstall python-rosinstall-generator python-wstool build-essential
测试下是否安装成功(小海龟实验)
打开终端(Termial),输入以下命令,初始化ROS环境
roscore
再 打开一个新的终端(Termial),输入以下命令,弹出一个小乌龟窗口:
rosrun turtlesim turtlesim_node
出现一个小乌龟的LOGO后,再 打开一个新的终端(Termial),输入以下命令
rosrun turtlesim turtle_teleop_key
然后,通过方向键控制小乌龟的移动
此时,说明安装ROS成功
13.安装 iai_kinect2
mkdir -p ~/catkin_ws/src
cd ~/catin_ws/src/
git clone https://github.com/code-iai/iai_kinect2.git
cd iai_kinect2
rosdep install -r --from-paths .
cd ~/catin_ws
catkin_make -DCMAKE_BUILD_TYPE="Release"
rospack profile
如果这里报错有许多未引用的错误,将build和devel删除重新编译即可
source ~/catkin_ws/devel/setup.bash
14.测试ros接口
roslaunch kinect2_bridge kinect2_bridge.launch
15.新开一个新的终端
rosrun kinect2_viewer kinect2_viewer
rosrun kinect2_viewer kinect2_viewer sd cloud
得到深度图后,需要得到人体的骨骼数据
1.NiViewer2
cd ~/libfreenect2/build
sudo apt-get install openni2-utils
cmake ..
make && sudo make install (optional)
sudo make install-openni2
NiViewer2
如果输出图像,则表示安装成功了。
生成的环境变量拷到.bashrc
cd ~
sudo gedit .bashrc
2.下载NiTE-2.0.0并解压链接:https://pan.baidu.com/s/1V9NW5fXmmxegIFiyZ9vfPA 提取码:m285
unzip NiTE文件.zip
tar -jxvf NiTE文件.bz2
sudo ./install.sh
生成NiTedevEnvironment文件
然后打开NiTE/Samples/Bin/,输入:
vim OpenNI.ini
加上一句Repository=/usr/lib/OpenNI2/Drivers(就是安装openni2生成的driver路径),然后在Samples/Bin路径下运行:
./UserViewer
如果不出意外的话便可以看到骨骼图了
3.安装Sensorkinect(optional)
解压并运行运行(与上述步骤相同):
./install.sh
即可完成安装。
4.编译并运行Openni2_tracker实现ros上面获取骨骼数据
在catkin_ws/src路径下,运行:
sudo git clone https://github.com/ros-drivers/openni2_tracker.git
下载Openni2_tracker包,打开CMakeLists.txt文件,修改路径(下面是我的放置路径)
# Find Nite2
message(status $ENV{NITE2_INCLUDE})
message(status $ENV{NITE2_REDIST64})
find_path(Nite2_INCLUDEDIR
NAMES NiTE.h
HINTS /home/software/NiTE-2.0.0/Include)
find_library(Nite2_LIBRARY
NAMES NiTE2
HINTS /home/software/NiTE-2.0.0/Redist
PATH_SUFFIXES lib)
修改完成会后,在catkin_ws路径下输入:
catkin_make
附上相应的代码
// openni_tracker.cpp
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <kdl/frames.hpp>
#include <XnOpenNI.h>
#include <XnCodecIDs.h>
#include <XnCppWrapper.h>
using std::string;
xn::Context g_Context;
xn::DepthGenerator g_DepthGenerator;
xn::UserGenerator g_UserGenerator;
XnBool g_bNeedPose = FALSE;//初始化,FALSE = 0,是否指定特定的姿势
XnChar g_strPose[20] = "";//姿势的名字
//检测到人
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
ROS_INFO("New User %d,g_bNeedPose = %d", nId,g_bNeedPose);
//此时g_bNeedPose = TRUE;g_strPose = psi;
if (g_bNeedPose)
//检测特定的用户姿势,此时UserPose_PoseDetected 回调函数执行
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
else
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
//检测不到人
void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
ROS_INFO("Lost user %d", nId);
}
//开始校准
void XN_CALLBACK_TYPE UserCalibration_CalibrationStart(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie) {
ROS_INFO("Calibration started for user %d", nId);
}
//校准结束
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) {
if (bSuccess) {//校准成功,开始追踪骨架
ROS_INFO("Calibration complete, start tracking user %d", nId);
g_UserGenerator.GetSkeletonCap().StartTracking(nId);
}
else {//失败,重新开始检测姿势
ROS_INFO("Calibration failed for user %d", nId);
if (g_bNeedPose)
g_UserGenerator.GetPoseDetectionCap().StartPoseDetection(g_strPose, nId);
else
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);//TRUE 忽略以前的校准以强制进一步校准。
}
}
//检测姿势
void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, XnChar const* strPose, XnUserID nId, void* pCookie) {
ROS_INFO("Pose %s detected for user %d", strPose, nId);
g_UserGenerator.GetPoseDetectionCap().StopPoseDetection(nId);
//开始校准
g_UserGenerator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
//TF 转换
void publishTransform(XnUserID const& user, XnSkeletonJoint const& joint, string const& frame_id, string const& child_frame_id) {
static tf::TransformBroadcaster br;
XnSkeletonJointPosition joint_position;//特定关节的位置。结构体,包含世界坐标和置信度
//获取最近生成的用户数据中的骨架关节之一的位置。
g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
double x = -joint_position.position.X / 1000.0;
double y = joint_position.position.Y / 1000.0;
double z = joint_position.position.Z / 1000.0;
XnSkeletonJointOrientation joint_orientation; //特定关节的方向。结构体中包含 方向和置信度
//获取特定关节的方向
g_UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);
XnFloat* m = joint_orientation.orientation.elements;
KDL::Rotation rotation(m[0], m[1], m[2],
m[3], m[4], m[5],
m[6], m[7], m[8]);
double qx, qy, qz, qw;
//获取此矩阵的四元数
rotation.GetQuaternion(qx, qy, qz, qw);
char child_frame_no[128];
snprintf(child_frame_no, sizeof(child_frame_no), "%s_%d", child_frame_id.c_str(), user);
tf::Transform transform;
//设置平移元素
transform.setOrigin(tf::Vector3(x, y, z));
//通过四元数设置旋转元素
transform.setRotation(tf::Quaternion(qx, -qy, -qz, qw));
// #4994 基准点(摄像头位置)
tf::Transform change_frame;
change_frame.setOrigin(tf::Vector3(0, 0, 0));
tf::Quaternion frame_rotation;
frame_rotation.setEulerZYX(1.5708, 0, 1.5708);
change_frame.setRotation(frame_rotation);
transform = change_frame * transform;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_no));
}
void publishTransforms(const std::string& frame_id) {
XnUserID users[15];
XnUInt16 users_count = 15;
g_UserGenerator.GetUsers(users, users_count);
for (int i = 0; i < users_count; ++i) {
XnUserID user = users[i];
//
if (!g_UserGenerator.GetSkeletonCap().IsTracking(user))
continue;
publishTransform(user, XN_SKEL_HEAD, frame_id, "head");
publishTransform(user, XN_SKEL_NECK, frame_id, "neck");
publishTransform(user, XN_SKEL_TORSO, frame_id, "torso");
publishTransform(user, XN_SKEL_LEFT_SHOULDER, frame_id, "left_shoulder");
publishTransform(user, XN_SKEL_LEFT_ELBOW, frame_id, "left_elbow");
publishTransform(user, XN_SKEL_LEFT_HAND, frame_id, "left_hand");
publishTransform(user, XN_SKEL_RIGHT_SHOULDER, frame_id, "right_shoulder");
publishTransform(user, XN_SKEL_RIGHT_ELBOW, frame_id, "right_elbow");
publishTransform(user, XN_SKEL_RIGHT_HAND, frame_id, "right_hand");
publishTransform(user, XN_SKEL_LEFT_HIP, frame_id, "left_hip");
publishTransform(user, XN_SKEL_LEFT_KNEE, frame_id, "left_knee");
publishTransform(user, XN_SKEL_LEFT_FOOT, frame_id, "left_foot");
publishTransform(user, XN_SKEL_RIGHT_HIP, frame_id, "right_hip");
publishTransform(user, XN_SKEL_RIGHT_KNEE, frame_id, "right_knee");
publishTransform(user, XN_SKEL_RIGHT_FOOT, frame_id, "right_foot");
}
}
#define CHECK_RC(nRetVal, what) \
if (nRetVal != XN_STATUS_OK) \
{ \
ROS_ERROR("%s failed: %s", what, xnGetStatusString(nRetVal));\
return nRetVal; \
}else{ \
ROS_INFO("%s OK: %s", what, xnGetStatusString(nRetVal)) ; \
} \
int main(int argc, char **argv) {
ros::init(argc, argv, "openni_tracker");
ros::NodeHandle nh;
//配置文件的路径
string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml";
ROS_INFO("configName==%s",configFilename.c_str());
XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
CHECK_RC(nRetVal, "InitFromXml");
/*搜索指定类型的现有已创建节点并返回其引用。
* 参数1:指定搜索的类型
* 参数2:现有已创建节点的引用
*/
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_DEPTH, g_DepthGenerator);
CHECK_RC(nRetVal, "Find depth generator");
nRetVal = g_Context.FindExistingNode(XN_NODE_TYPE_USER, g_UserGenerator);
if (nRetVal != XN_STATUS_OK) {
nRetVal = g_UserGenerator.Create(g_Context);
if (nRetVal != XN_STATUS_OK) {
ROS_ERROR("NITE is likely missing: Please install NITE >= 1.5.2.21. Check the readme for download information. Error Info: User generator failed: %s", xnGetStatusString(nRetVal));
return nRetVal;
}
}
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
ROS_INFO("Supplied user generator doesn't support skeleton");
return 1;
}
XnCallbackHandle hUserCallbacks;
/*“新用户”和“失去用户”事件的注册。
参数:User_NewUser,检测到新用户回调函数
参数:User_LostUser,检测到失去用户回调
*/
g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
XnCallbackHandle hCalibrationCallbacks;
/*注册校准开始和结束事件
* 参数:UserCalibration_CalibrationStart ,校准开始回调函数
* 参数:UserCalibration_CalibrationEnd ,校准结束回调函数
*/
g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
//是否需要对特定姿势进行校准,适用于所有用户
if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) {
ROS_INFO("g_bNeedPose = TRUE");
//此处将g_bNeedPose 赋值 1;
g_bNeedPose = TRUE;
//是否支持特殊姿势校准
if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
ROS_INFO("Pose required, but not supported");
return 1;
}
XnCallbackHandle hPoseCallbacks;
/*注册检测姿势事件
* 参数:UserPose_PoseDetected 开始检测姿势回调函数
* 第二个参数:检测姿势结束回调函数
* */
g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
ROS_INFO("NAME1==%s",g_strPose);
/**
* 此方法仅在NeedPoseForCalibration()方法返回TRUE时使用,
* 并且返回姿势名称。
*/
g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
ROS_INFO("NAME2==%s",g_strPose);
}
/*
* 设置骨架轮廓。 骨架配置文件指定哪些关节处于活动状态,哪些关节处于非活动状态。
* UserGenerator节点仅为活动关节生成输出数据。
* 此配置文件适用于@ref UserGenerator节点生成的所有骨架。
* 参数:指定要设置的配置文件
* 配置文件作用是使程序能够选择生成所有关节,还是只是上或下躯干,或只是头和手。
* 使用SetJointActive()方法选择配置文件,使其具有比此方法更好的识别率,例如,能够选择特定的手或脚。
* 这个函数只提供程序需要的数据,因此执行效率和相应时间由很大的提升
* */
g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
//确保所有创建的@ref dict_gen节点“生成器节点”正在生成数据
nRetVal = g_Context.StartGeneratingAll();
CHECK_RC(nRetVal, "StartGenerating");
ros::Rate r(30);
ros::NodeHandle pnh("~");
string frame_id("xtion_depth_frame");
pnh.getParam("camera_frame_id", frame_id);
while (ros::ok()) {
/*将上下文中的所有生成器节点更新为其最新的可用数据,
* 等待所有节点有新的数据可用。我们要确保所有创建的生成器节点正在生成数据,不然会一直在等待
*/
g_Context.WaitAndUpdateAll();
publishTransforms(frame_id);
r.sleep();
}
g_Context.Shutdown();
return 0;
}
如若需要,看程序进行更改。
报错:
Could not find a package configuration file provided by “orocos_kdl” with
any of the following names:
orocos_kdlConfig.cmake
orocos_kdl-config.cmake
将openni2_tracker下面CMakeLists.txt文件中下面这个命令
find_package(catkin REQUIRED COMPONENTS orocos_kdl )
改为:
find_package(catkin REQUIRED COMPONENTS )
find_package(orocos_kdl REQUIRED)
追踪流程大体如下:
1.注册检测到人和人消失的回调函数
2.注册注册检测姿势,校准开始结束回调函数
3.判断是否需要检测特定姿势,是否支持该功能
4.获取姿势的名字(”Psi”)
5.设置骨架轮廓配置。
6.启动所有生成器节点
7.等待所有节点有新的数据可用。期间执行相应的回调函数(前面注册过)。
7.1 检测到user,开始姿势检测
7.2 姿势检测到之后开始校准
7.3 校准成功,开始追踪,失败则重新检测姿势。
8.tf转换,在rviz中显示。
5.测试:
Terminal_1:
roslaunch openni2_launch openni2.launch
报错:Unsupported IR video mode - Resolution: [email protected] Format: Gray16,这个没有关系,只是表示当前的这个不支持color和ir图像模式,出现这个提示正常。
Terminal_2:这个~/NiTE-2.0.0/Redist终端目录下面执行
rosrun openni2_tracker openni2_tracker
Terminal_3:
rosrun rviz rviz
即可看到骨骼数据。