jsk-ros-pkg · ishiguroJSK · May 24, 2016 · May 24, 2016 · May 24, 2016 · May 24, 2016
diff --git a/openni2_tracker/.gitignore b/openni2_tracker/.gitignore
@@ -0,0 +1,40 @@
+# Eclipse project files
+.project
+.cproject
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# gedit
+*~
+
+# NiTE
+NiTE-2.0.0
+NiTE-Linux-x64-2.0.0.tar.bz2
diff --git a/openni2_tracker/CMakeLists.txt b/openni2_tracker/CMakeLists.txt
@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(openni2_tracker)
+find_package(catkin REQUIRED COMPONENTS geometry_msgs
+					#orocos_kdl
+					roscpp
+					roslib
+					tf)
+#find_package(orocos_kdl)
+# Find OpenNI2
+#find_package(PkgConfig)
+#pkg_check_modules(OpenNI2 REQUIRED libopenni2)
+find_path(OpenNI2_INCLUDEDIR
+	  NAMES OpenNI.h
+	  HINTS /usr/include/openni2)
+find_library(OpenNI2_LIBRARIES
+	     NAMES OpenNI2 DummyDevice OniFile PS1090
+	     HINTS /usr/lib/ /usr/lib/OpenNI2/Drivers
+	     PATH_SUFFIXES lib) 
+message(STATUS ${OpenNI2_LIBRARIES})
+
+#extract NiTE
+add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/NiTE-2.0.0/NiTEDevEnvironment
+  DEPENDS NiTE-Linux-x64-2.0.0.tar.bz2
+  COMMAND tar -xvjf ${PROJECT_SOURCE_DIR}/NiTE-Linux-x64-2.0.0.tar.bz2
+  COMMAND cd NiTE-2.0.0 && ./install.sh
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR})
+add_custom_target(install_nite2 ALL
+  DEPENDS  ${PROJECT_SOURCE_DIR}/NiTE-2.0.0/NiTEDevEnvironment)
+
+
+
+# Find Nite2
+message(status $ENV{NITE2_INCLUDE})
+message(status $ENV{NITE2_REDIST64})
+find_path(Nite2_INCLUDEDIR
+	  NAMES NiTE.h
+	  HINTS $ENV{NITE2_INCLUDE})
+find_library(Nite2_LIBRARY
+	     NAMES NiTE2
+	     HINTS $ENV{NITE2_REDIST64}
+	     PATH_SUFFIXES lib) 
+
+catkin_package()
+
+include_directories(${catkin_INCLUDEDIR}
+		    ${OpenNI2_INCLUDEDIR}
+		    ${Nite2_INCLUDEDIR})
+add_executable(openni2_tracker src/openni2_tracker.cpp)
+target_link_libraries(openni2_tracker ${catkin_LIBRARIES} ${OpenNI2_LIBRARIES} ${Nite2_LIBRARY})
+
+#install NiTE?
+install(DIRECTORY Bin
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+   USE_SOURCE_PERMISSIONS
+ )
+
+install(TARGETS openni2_tracker RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+install(FILES openni2_tracker.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/openni2_tracker/README.md b/openni2_tracker/README.md
@@ -0,0 +1,31 @@
+# OpenNI2 Tracker
+
+#!! This Package Needs Unprovided External Library "NiTE2" !!
+
+Now(2016.5), the NiTE2 library was not provided in official.   
+Please get NiTE2 library file named like "NiTE-Linux-x64-2.0.0.tar.bz2" in some other way...   
+
+If you can get the NiTE2 library, put it into the top of this package   
+`(CATKIN_WS)/src/jsk-ros-pkg/jsk_openni_kinect/openni2_tracker/NiTE-Linux-x64-2.0.0.tar.bz2`   
+and then, run `catkin build`.
+
+This node cannot be run from `rosrun` because of a troublesome runtime path dependency of NiTE2,   
+So please launch like   
+`roslaunch openni2_tracker tracker.launch`  
+and you'll see
+```
+Warning: USB events thread - failed to set priority. This might cause loss of data...
+
+Start moving around to get detected...
+(PSI pose may be required for skeleton calibration, depending on the configuration)
+[02068893] User #1:	New
+[ INFO] [1464110006.050095646]: Found a new user.
+[02102262] User #1:	Calibrating...
+[03703986] User #1:	Tracking!
+[ INFO] [1464110007.677402500]: Now tracking user 1
+[ INFO] [1464110007.711739009]: Now tracking user 1
+[ INFO] [1464110007.743820543]: Now tracking user 1
+[ INFO] [1464110007.774881167]: Now tracking user 1
+[ INFO] [1464110007.810146388]: Now tracking user 1
+[ INFO] [1464110007.842145840]: Now tracking user 1
+```
diff --git a/openni2_tracker/launch/tracker.launch b/openni2_tracker/launch/tracker.launch
@@ -0,0 +1,17 @@
+<!-- openni2_tracker Launch File -->
+<launch>
+
+  <arg name="tracker_name" default="tracker" />
+
+  <node name="openni_tracker" pkg="openni2_tracker" type="openni2_tracker" launch-prefix="sh $(find openni2_tracker)/run" output="screen">
+<!--    <param name="tf_prefix" value="$(arg tracker_name)" />-->
+    <param name="camera_frame" value="/$(arg tracker_name)_depth_frame" />
+  </node>
+
+  <!-- TF Static Transforms to World -->
+<!--  <node pkg="tf" type="static_transform_publisher" name="world_to_tracker" args=" 0 0 1.25 1.5707 0 1.7707  /world /$(arg tracker_name)_depth_frame 100"/> -->
+
+  <!-- Example tf config : Human's cood rotation will be same as world cood, and camera and human will face each other-->
+  <node pkg="tf" type="static_transform_publisher" name="world_to_tracker" args=" 2.0 0.0 1.0 3.1415 0 0 /world /$(arg tracker_name)_depth_frame 100"/> 
+
+</launch>
diff --git a/openni2_tracker/package.xml b/openni2_tracker/package.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package>
+  <name>openni2_tracker</name>
+  <version>0.1.0</version>
+  <description>
+    The openni_tracker broadcasts the OpenNI skeleton frames using tf.
+    This new version of the openni_tracker uses OpenNI2 and Nite2</description>
+  <maintainer email="ishiguro@jsk.imi.i.u-tokyo.ac.jp">ishiguroJSK</maintainer>
+  <license>BSD</license>
+  <url type="website">http://www.ros.org/wiki/openni_tracker</url>  
+  <author email="marcus.liebhardt@yujinrobot.com">Marcus Liebhardt</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>libopenni2-dev</build_depend>
+  <build_depend>libusb-1.0-dev</build_depend>
+<!--  <build_depend>libopenni-nite-dev</build_depend> -->
+  <build_depend>libopenni-sensor-primesense-dev</build_depend> -->
+  <build_depend>geometry_msgs</build_depend>
+<!--  <build_depend>orocos_kdl</build_depend>-->
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tar</build_depend>
+
+  <run_depend>libopenni2-dev</run_depend>
+  <run_depend>libusb-1.0-dev</run_depend>
+<!--  <run_depend>libopenni-nite-dev</run_depend> -->
+  <run_depend>libopenni-sensor-primesense-dev</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+<!--  <run_depend>orocos_kdl</run_depend>-->
+  <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>tf</run_depend>
+</package>
diff --git a/openni2_tracker/run b/openni2_tracker/run
@@ -0,0 +1,4 @@
+#!/bin/bash
+
+cd ~/NiTE-2.0.0/Redist
+exec $@
diff --git a/openni2_tracker/src/openni2_tracker.cpp b/openni2_tracker/src/openni2_tracker.cpp
@@ -0,0 +1,144 @@
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <tf/transform_broadcaster.h>
+//#include <kdl/frames.hpp>
+
+//#include <OpenNI.h>
+//#include <XnCodecIDs.h>
+//#include <XnCppWrapper.h>
+#include <NiTE.h>
+
+using std::string;
+
+#define MAX_USERS 10
+bool g_visibleUsers[MAX_USERS] = {false};
+nite::SkeletonState g_skeletonStates[MAX_USERS] = {nite::SKELETON_NONE};
+
+#define USER_MESSAGE(msg) \
+	{printf("[%08llu] User #%d:\t%s\n",ts, user.getId(),msg);}
+
+
+void updateUserState(const nite::UserData& user, unsigned long long ts)
+{
+	if (user.isNew())
+		USER_MESSAGE("New")
+	else if (user.isVisible() && !g_visibleUsers[user.getId()])
+		USER_MESSAGE("Visible")
+	else if (!user.isVisible() && g_visibleUsers[user.getId()])
+		USER_MESSAGE("Out of Scene")
+	else if (user.isLost())
+		USER_MESSAGE("Lost")
+
+	g_visibleUsers[user.getId()] = user.isVisible();
+
+
+	if(g_skeletonStates[user.getId()] != user.getSkeleton().getState())
+	{
+		switch(g_skeletonStates[user.getId()] = user.getSkeleton().getState())
+		{
+		case nite::SKELETON_NONE:
+			USER_MESSAGE("Stopped tracking.")
+			break;
+		case nite::SKELETON_CALIBRATING:
+			USER_MESSAGE("Calibrating...")
+			break;
+		case nite::SKELETON_TRACKED:
+			USER_MESSAGE("Tracking!")
+			break;
+		case nite::SKELETON_CALIBRATION_ERROR_NOT_IN_POSE:
+		case nite::SKELETON_CALIBRATION_ERROR_HANDS:
+		case nite::SKELETON_CALIBRATION_ERROR_LEGS:
+		case nite::SKELETON_CALIBRATION_ERROR_HEAD:
+		case nite::SKELETON_CALIBRATION_ERROR_TORSO:
+			USER_MESSAGE("Calibration Failed... :-|")
+			break;
+		}
+	}
+}
+
+int main(int argc, char **argv) {
+
+    ros::init(argc, argv, "openni_tracker");
+    ros::NodeHandle nh, nh_priv("~");
+
+    nite::UserTracker userTracker;
+    nite::Status niteRc;
+
+    nite::NiTE::initialize();
+
+	ros::Rate r(30);
+    std::string cam_frame_name;
+    nh_priv.getParam("camera_frame", cam_frame_name);
+
+    static tf::TransformBroadcaster br;
+    tf::Transform transform;
+    tf::Quaternion q;
+    const std::string jnames[] = {//This is converted from NiteEnums.h
+    	"JOINT_HEAD",
+    	"JOINT_NECK",
+    	"JOINT_LEFT_SHOULDER",
+    	"JOINT_RIGHT_SHOULDER",
+    	"JOINT_LEFT_ELBOW",
+    	"JOINT_RIGHT_ELBOW",
+    	"JOINT_LEFT_HAND",
+    	"JOINT_RIGHT_HAND",
+    	"JOINT_TORSO",
+    	"JOINT_LEFT_HIP",
+    	"JOINT_RIGHT_HIP",
+    	"JOINT_LEFT_KNEE",
+    	"JOINT_RIGHT_KNEE",
+    	"JOINT_LEFT_FOOT",
+    	"JOINT_RIGHT_FOOT",
+    };
+    const std::vector<std::string> jnamelist(jnames,jnames+15);
+
+	niteRc = userTracker.create();
+	if (niteRc != nite::STATUS_OK)
+	{
+		printf("Couldn't create user tracker\n");
+		return 3;
+	}
+	printf("\nStart moving around to get detected...\n(PSI pose may be required for skeleton calibration, depending on the configuration)\n");
+
+	nite::UserTrackerFrameRef userTrackerFrame;
+
+	while (ros::ok())
+	{
+		niteRc = userTracker.readFrame(&userTrackerFrame);
+		if (niteRc == nite::STATUS_OK)
+		{
+			const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers();
+			for (int i = 0; i < users.getSize(); ++i)
+			{
+				const nite::UserData& user = users[i];
+				updateUserState(user,userTrackerFrame.getTimestamp());
+				if (user.isNew())
+				{
+					userTracker.startSkeletonTracking(user.getId());
+					ROS_INFO_STREAM("Found a new user.");
+				}
+				else if (user.getSkeleton().getState() == nite::SKELETON_TRACKED)
+				{
+					ROS_INFO_STREAM("Now tracking user " << user.getId());
+					for(int i=0;i<jnamelist.size();i++){
+						const nite::SkeletonJoint& cur_joint = user.getSkeleton().getJoint(static_cast<nite::JointType>(i));
+//						if (cur_joint.getPositionConfidence() > 0.5){
+							transform.setOrigin( tf::Vector3(cur_joint.getPosition().z/1000.0, cur_joint.getPosition().x/1000.0, cur_joint.getPosition().y/1000.0) );//XtionData = Left:X, Up:Y, Forward:Z
+							q.setRPY(0, 0, M_PI);// Assuming human is in front of and face to camera device (Human's forward:X Left:Y Up:Z)
+							transform.setRotation(q);
+						    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), cam_frame_name, jnamelist[i]));
+//						}
+					}
+				}
+			}
+		}
+		else
+		{
+			ROS_WARN_STREAM("Get next frame failed.");
+		}
+
+		r.sleep();
+	}
+	return 0;
+}
+