diff --git a/CMakeLists.txt b/CMakeLists.txt
index 094802d..b5969b6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,39 +1,113 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(openni2_tracker)
-find_package(catkin REQUIRED COMPONENTS geometry_msgs
-					orocos_kdl
+find_package(catkin REQUIRED COMPONENTS 
 					roscpp
 					roslib
 					tf)
-# 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})
-
-# 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) 
+find_package(orocos_kdl REQUIRED)
 
 catkin_package()
 
-include_directories(${catkin_INCLUDEDIR}
-		    ${OpenNI2_INCLUDEDIR}
-		    ${Nite2_INCLUDEDIR})
+include(ExternalProject)
+## Check architecture 32 or 64 bit for precompiled OpenNI2 and NiTE2 Libraries
+if (CMAKE_SIZEOF_VOID_P MATCHES "8")
+	ExternalProject_Add(
+		OpenNI2
+		PREFIX ${CMAKE_CURRENT_BINARY_DIR}
+		URL http://www.openni.ru/wp-content/uploads/2013/01/OpenNI-Linux-x64-2.1.0.tar.zip
+		URL_MD5 be1f12b964fe07f4a3243b946c6ce387
+		UPDATE_COMMAND tar -xf <SOURCE_DIR>/OpenNI-Linux-x64-2.1.0.tar.bz2
+		CONFIGURE_COMMAND ""
+		BUILD_COMMAND ""
+	)
+	ExternalProject_Add(
+		NiTE2
+		PREFIX ${CMAKE_CURRENT_BINARY_DIR}
+		URL http://www.openni.ru/wp-content/uploads/NiTE-Linux-x64-2.0.0.tar.zip
+		URL_MD5 4244e25bebc87a15fc761b7371107494
+		UPDATE_COMMAND tar -xf <SOURCE_DIR>/NiTE-Linux-x64-2.0.0.tar.bz2
+		CONFIGURE_COMMAND ""
+		BUILD_COMMAND ""
+	)
+
+	add_custom_target(OpenNI2_copy)
+	add_dependencies(OpenNI2_copy OpenNI2)
+	add_custom_command(TARGET OpenNI2_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/OpenNI2/OpenNI-2.1.0-x64 ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x64)
+
+	add_custom_target(NiTE2_copy)
+	add_dependencies(NiTE2_copy NiTE2)
+	add_custom_command(TARGET NiTE2_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/NiTE2/NiTE-2.0.0 		${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/NiTE-2.0.0)
+
+	add_custom_target(OpenNI2_conf_copy)
+	add_dependencies(OpenNI2_conf_copy OpenNI2)
+	add_custom_command(TARGET OpenNI2_conf_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/OpenNI2/OpenNI-2.1.0-x64/Redist/OpenNI2	$ENV{HOME}/.ros/OpenNI2)
+
+	add_custom_target(NiTE2_conf_copy)
+	add_dependencies(NiTE2_conf_copy NiTE2)
+	add_custom_command(TARGET NiTE2_conf_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/NiTE2/NiTE-2.0.0/Redist/NiTE2	$ENV{HOME}/.ros/NiTE2)
+
+	set(OpenNI2_INCLUDEDIR 	${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x64/Include)
+	set(OpenNI2_LIBRARIES 	${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x64/Redist/libOpenNI2.so
+	 						${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x64/Redist/OpenNI2/Drivers/libPS1080.so
+	 						${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x64/Redist/OpenNI2/Drivers/libOniFile.so)
+	set(Nite2_INCLUDEDIR 	${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/NiTE-2.0.0/Include)
+	set(Nite2_LIBRARY 		${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/NiTE-2.0.0/Redist/libNiTE2.so)
+elseif (CMAKE_SIZEOF_VOID_P MATCHES "4")
+	ExternalProject_Add(
+		OpenNI2
+		PREFIX ${CMAKE_CURRENT_BINARY_DIR}
+		URL http://www.openni.ru/wp-content/uploads/2013/01/OpenNI-Linux-x86-2.1.0.tar.zip
+		URL_MD5 827b37db761b5885177d3e414cec7762
+		UPDATE_COMMAND tar -xf <SOURCE_DIR>/OpenNI-Linux-x86-2.1.0.tar.bz2
+		CONFIGURE_COMMAND ""
+		BUILD_COMMAND ""
+	)
+	ExternalProject_Add(
+		NiTE2
+		PREFIX ${CMAKE_CURRENT_BINARY_DIR}
+		URL http://www.openni.ru/wp-content/uploads/NiTE-Linux-x86-2.0.0.tar.zip
+		URL_MD5 30d66a07447a73e5bcd8f28af6628606
+		UPDATE_COMMAND tar -xf <SOURCE_DIR>/NiTE-Linux-x86-2.0.0.tar.bz2
+		CONFIGURE_COMMAND ""
+		BUILD_COMMAND ""
+	)
+
+	add_custom_target(OpenNI2_copy)
+	add_dependencies(OpenNI2_copy OpenNI2)
+	add_custom_command(TARGET OpenNI2_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/OpenNI2/OpenNI-2.1.0-x86 ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x86)
+
+	add_custom_target(NiTE2_copy)
+	add_dependencies(NiTE2_copy NiTE2)
+	add_custom_command(TARGET NiTE2_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/NiTE2/NiTE-2.0.0 		${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/NiTE-2.0.0)
+
+	add_custom_target(OpenNI2_conf_copy)
+	add_dependencies(OpenNI2_conf_copy OpenNI2)
+	add_custom_command(TARGET OpenNI2_conf_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/OpenNI2/OpenNI-2.1.0-x86/Redist/OpenNI2	$ENV{HOME}/.ros/OpenNI2)
+
+	add_custom_target(NiTE2_conf_copy)
+	add_dependencies(NiTE2_conf_copy NiTE2)
+	add_custom_command(TARGET NiTE2_conf_copy PRE_BUILD
+						COMMAND ${CMAKE_COMMAND} -E copy_directory ${CMAKE_CURRENT_BINARY_DIR}/src/NiTE2/NiTE-2.0.0/Redist/NiTE2	$ENV{HOME}/.ros/NiTE2)
+
+	set(OpenNI2_INCLUDEDIR 	${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x86/Include)
+	set(OpenNI2_LIBRARIES 	${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x86/Redist/libOpenNI2.so
+	 						${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x86/Redist/OpenNI2/Drivers/libPS1080.so
+	 						${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/OpenNI-2.1.0-x86/Redist/OpenNI2/Drivers/libOniFile.so)
+	set(Nite2_INCLUDEDIR 	${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/NiTE-2.0.0/Include)
+	set(Nite2_LIBRARY 		${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_BIN_DESTINATION}/NiTE-2.0.0/Redist/libNiTE2.so)
+endif (CMAKE_SIZEOF_VOID_P MATCHES "8")
+
+include_directories(${catkin_INCLUDEDIR} ${OpenNI2_INCLUDEDIR} ${Nite2_INCLUDEDIR})
+
 add_executable(openni2_tracker src/openni2_tracker.cpp)
+add_dependencies(openni2_tracker OpenNI2_copy NiTE2_copy OpenNI2_conf_copy NiTE2_conf_copy)
 target_link_libraries(openni2_tracker ${catkin_LIBRARIES} ${OpenNI2_LIBRARIES} ${Nite2_LIBRARY})
 
 install(TARGETS openni2_tracker RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/launch/openni2_tracker.launch b/launch/openni2_tracker.launch
new file mode 100644
index 0000000..15e20fd
--- /dev/null
+++ b/launch/openni2_tracker.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+	<arg name="tracker_name" default="tracker" />
+	<!-- Load the Tracker -->
+	<node name="openni2_tracker" pkg="openni2_tracker" type="openni2_tracker" >
+		<param name="tf_prefix" value="$(arg tracker_name)" />
+		<param name="relative_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"/> 
+</launch>
\ No newline at end of file
diff --git a/package.xml b/package.xml
index a00c165..c2ff3ba 100644
--- a/package.xml
+++ b/package.xml
@@ -14,7 +14,6 @@
 
   <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>
@@ -24,7 +23,6 @@
 
   <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>
diff --git a/src/openni2_tracker.cpp b/src/openni2_tracker.cpp
index 71ca235..5cb6dff 100644
--- a/src/openni2_tracker.cpp
+++ b/src/openni2_tracker.cpp
@@ -35,144 +35,22 @@
 #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;
-
-//xn::Context        g_Context;
-//xn::DepthGenerator g_DepthGenerator;
-//xn::UserGenerator  g_UserGenerator;
+using namespace nite;
 
 #define MAX_USERS 10
 bool g_visibleUsers[MAX_USERS] = {false};
-nite::SkeletonState g_skeletonStates[MAX_USERS] = {nite::SKELETON_NONE};
+SkeletonState g_skeletonStates[MAX_USERS] = {SKELETON_NONE};
 
 #define USER_MESSAGE(msg) \
 	{printf("[%08llu] User #%d:\t%s\n",ts, user.getId(),msg);}
 
-//XnBool g_bNeedPose   = FALSE;
-//XnChar g_strPose[20] = "";
-//
-//void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
-//	ROS_INFO("New User %d", nId);
-//
-//	if (g_bNeedPose)
-//		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);
-//	}
-//}
-//
-//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);
-//}
-//
-//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;												\
-//	}
+typedef std::map<string, SkeletonJoint> JointMap;
 
-void updateUserState(const nite::UserData& user, unsigned long long ts)
+void updateUserState(const UserData &user,
+                     unsigned long long ts)
 {
 	if (user.isNew())
 		USER_MESSAGE("New")
@@ -190,115 +68,130 @@ void updateUserState(const nite::UserData& user, unsigned long long ts)
 	{
 		switch(g_skeletonStates[user.getId()] = user.getSkeleton().getState())
 		{
-		case nite::SKELETON_NONE:
+    case SKELETON_NONE:
 			USER_MESSAGE("Stopped tracking.")
 			break;
-		case nite::SKELETON_CALIBRATING:
+    case SKELETON_CALIBRATING:
 			USER_MESSAGE("Calibrating...")
 			break;
-		case nite::SKELETON_TRACKED:
+    case 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:
+    case SKELETON_CALIBRATION_ERROR_NOT_IN_POSE:
+    case SKELETON_CALIBRATION_ERROR_HANDS:
+    case SKELETON_CALIBRATION_ERROR_LEGS:
+    case SKELETON_CALIBRATION_ERROR_HEAD:
+    case SKELETON_CALIBRATION_ERROR_TORSO:
 			USER_MESSAGE("Calibration Failed... :-|")
 			break;
 		}
 	}
 }
 
-int main(int argc, char **argv) {
-//    string configFilename = ros::package::getPath("openni_tracker") + "/openni_tracker.xml";
-//    XnStatus nRetVal = g_Context.InitFromXmlFile(configFilename.c_str());
-//    CHECK_RC(nRetVal, "InitFromXml");
-//
-//    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);
-//		CHECK_RC(nRetVal, "Find user generator");
-//	}
-//
-//	if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {
-//		ROS_INFO("Supplied user generator doesn't support skeleton");
-//		return 1;
-//	}
-//
-//    XnCallbackHandle hUserCallbacks;
-//	g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
-//
-//	XnCallbackHandle hCalibrationCallbacks;
-//	g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);
-//
-//	if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration()) {
-//		g_bNeedPose = TRUE;
-//		if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION)) {
-//			ROS_INFO("Pose required, but not supported");
-//			return 1;
-//		}
-//
-//		XnCallbackHandle hPoseCallbacks;
-//		g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
-//
-//		g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
-//	}
-//
-//	g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
-//
-//	nRetVal = g_Context.StartGeneratingAll();
-//	CHECK_RC(nRetVal, "StartGenerating");
+bool publishJointTF(const string &j_name,
+                    const SkeletonJoint &j,
+                    const string &tf_prefix,
+                    const string &relative_frame,
+                    const int &uid,
+                    tf::TransformBroadcaster &br)
+{
+	if (j.getPositionConfidence() > 0.0){
+	  tf::Transform transform;
+	  transform.setOrigin(tf::Vector3(j.getPosition().x/1000.0, j.getPosition().y/1000.0, j.getPosition().z/1000.0));
+	  transform.setRotation(tf::Quaternion(0, 0, 0, 1));
+    std::stringstream frame_id_stream;
+    string frame_id;
+	  frame_id_stream << "/" << tf_prefix << "/user_" << uid << "/" << j_name;
+	  frame_id = frame_id_stream.str();
+	  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), relative_frame, frame_id));
+	}
+	return true;
+}
 
-    ros::init(argc, argv, "openni_tracker");
-    ros::NodeHandle nh, nh_priv("~");
+int main(int argc, char **argv) 
+{
+	ros::init(argc, argv, "openni_tracker");
+	ros::NodeHandle nh, nh_priv("~");
+	tf::TransformBroadcaster br;
+  string tf_prefix, relative_frame = "";
+
+	openni::Status rc = openni::STATUS_OK;
+	rc = openni::OpenNI::initialize();
+	if (rc != openni::STATUS_OK)
+	{
+	  ROS_ERROR("Failed to initialize OpenNI\n%s\n", openni::OpenNI::getExtendedError());
+	  return 4;
+	}
+	
+  UserTracker userTracker;
+  Status niteRc;
 
-    nite::UserTracker userTracker;
-    nite::Status niteRc;
+	// Get Tracker Parameters
+	if(!nh_priv.getParam("tf_prefix", tf_prefix)){
+		ROS_ERROR("tf_prefix not found on Param Server! Check your launch file!");
+	}
+	if(!nh_priv.getParam("relative_frame", relative_frame)){
+		ROS_ERROR("relative_frame not found on Param Server! Check your launch file!");
+	}
 
-    nite::NiTE::initialize();
+  NiTE::initialize();
 
 	ros::Rate r(30);
-    std::string frame_id("openni_depth_frame");
-    nh_priv.getParam("camera_frame_id", frame_id);
+  string frame_id("openni_depth_frame");
+	nh_priv.getParam("camera_frame_id", frame_id);
 
 	niteRc = userTracker.create();
-	if (niteRc != nite::STATUS_OK)
+  if (niteRc != STATUS_OK)
 	{
-		printf("Couldn't create user tracker\n");
+    printf("Couldn't create user tracker\n%s\n", openni::OpenNI::getExtendedError());
 		return 3;
 	}
-	printf("\nStart moving around to get detected...\n(PSI pose may be required for skeleton calibration, depending on the configuration)\n");
+  ROS_INFO_STREAM("\nStart moving around to get detected...\n(PSI pose may be required for skeleton calibration, depending on the configuration)\n");
 
-	nite::UserTrackerFrameRef userTrackerFrame;
+  UserTrackerFrameRef userTrackerFrame;
 
 	while (ros::ok())
 	{
 		niteRc = userTracker.readFrame(&userTrackerFrame);
-		if (niteRc == nite::STATUS_OK)
+    if (niteRc == STATUS_OK)
 		{
-			const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers();
+      const Array<UserData>& users = userTrackerFrame.getUsers();
 			for (int i = 0; i < users.getSize(); ++i)
 			{
-				const nite::UserData& user = users[i];
+        const 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)
+        else if (user.getSkeleton().getState() == SKELETON_TRACKED)
 				{
 					ROS_INFO_STREAM("Now tracking user " << user.getId());
-//					const nite::SkeletonJoint& head = user.getSkeleton().getJoint(nite::JOINT_HEAD);
-//					if (head.getPositionConfidence() > .5)
-//					printf("%d. (%5.2f, %5.2f, %5.2f)\n", user.getId(), head.getPosition().x, head.getPosition().y, head.getPosition().z);
+					JointMap named_joints;
+				
+          named_joints["head"] = (user.getSkeleton().getJoint(JOINT_HEAD) );
+          named_joints["neck"] = (user.getSkeleton().getJoint(JOINT_NECK) );
+          named_joints["left_shoulder"] = (user.getSkeleton().getJoint(JOINT_LEFT_SHOULDER) );
+          named_joints["right_shoulder"] = (user.getSkeleton().getJoint(JOINT_RIGHT_SHOULDER) );
+          named_joints["left_elbow"] = (user.getSkeleton().getJoint(JOINT_LEFT_ELBOW) );
+          named_joints["right_elbow"] = (user.getSkeleton().getJoint(JOINT_RIGHT_ELBOW) );
+          named_joints["left_hand"] = (user.getSkeleton().getJoint(JOINT_LEFT_HAND) );
+          named_joints["right_hand"] = (user.getSkeleton().getJoint(JOINT_RIGHT_HAND) );
+          named_joints["torso"] = (user.getSkeleton().getJoint(JOINT_TORSO) );
+          named_joints["left_hip"] = (user.getSkeleton().getJoint(JOINT_LEFT_HIP) );
+          named_joints["right_hip"] = (user.getSkeleton().getJoint(JOINT_RIGHT_HIP) );
+          named_joints["left_knee"] = (user.getSkeleton().getJoint(JOINT_LEFT_KNEE) );
+          named_joints["right_knee"] = (user.getSkeleton().getJoint(JOINT_RIGHT_KNEE) );
+          named_joints["left_foot"] = (user.getSkeleton().getJoint(JOINT_LEFT_FOOT) );
+          named_joints["right_foot"] = (user.getSkeleton().getJoint(JOINT_RIGHT_FOOT) );
+
+          for (JointMap::iterator it=named_joints.begin(); it!=named_joints.end(); ++it)
+          {
+            publishJointTF(it->first,it->second,tf_prefix,relative_frame,user.getId(),br);
+					}
 				}
 			}
-//			publishTransforms(frame_id);
 		}
 		else
 		{
@@ -307,5 +200,7 @@ int main(int argc, char **argv) {
 
 		r.sleep();
 	}
+
+  NiTE::shutdown();
 	return 0;
 }