diff --git a/CMakeLists.txt b/CMakeLists.txt index 094802d..69c713c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,25 +1,18 @@ cmake_minimum_required(VERSION 2.8.3) project(openni2_tracker) find_package(catkin REQUIRED COMPONENTS geometry_msgs - orocos_kdl roscpp roslib + nodelet + image_transport 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_package(PkgConfig) +pkg_check_modules(OpenNI2 REQUIRED libopenni2) # Find Nite2 -message(status $ENV{NITE2_INCLUDE}) -message(status $ENV{NITE2_REDIST64}) +message(STATUS "NITE2_INCLUDE: " $ENV{NITE2_INCLUDE}) +message(STATUS "NITE2_REDIST64: " $ENV{NITE2_REDIST64}) find_path(Nite2_INCLUDEDIR NAMES NiTE.h HINTS $ENV{NITE2_INCLUDE}) @@ -28,13 +21,24 @@ find_library(Nite2_LIBRARY HINTS $ENV{NITE2_REDIST64} PATH_SUFFIXES lib) -catkin_package() +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS nodelet roscpp +) -include_directories(${catkin_INCLUDEDIR} - ${OpenNI2_INCLUDEDIR} - ${Nite2_INCLUDEDIR}) -add_executable(openni2_tracker src/openni2_tracker.cpp) +include_directories(include + ${catkin_INCLUDEDIR} + ${OpenNI2_INCLUDE_DIRS} + ${Nite2_INCLUDEDIR}) +link_directories(${OpenNI2_LIBRARY_DIRS}) +add_library(openni2_tracker src/openni2_tracker_nodelet.cpp) target_link_libraries(openni2_tracker ${catkin_LIBRARIES} ${OpenNI2_LIBRARIES} ${Nite2_LIBRARY}) -install(TARGETS openni2_tracker RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install(TARGETS ${PROJECT_NAME} + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) install(FILES openni2_tracker.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES openni2_tracker_nodelet.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/README.md b/README.md index d69f662..4d6ca51 100644 --- a/README.md +++ b/README.md @@ -3,3 +3,33 @@ The OpenNI2 Tracker broadcasts the OpenNI skeleton frames using tf. For more information checkout the ROS Wiki: http://ros.org/wiki/openni2_tracker. OpenNI2 Tracker is the new version of the http://github.com/ros-drivers/openni_tracker using OpenNI2 and Nite2. + +# How to install NiTE2 + +1. put NiTE-Linux-x64-2.0.0.tar.bz2.gz under ~/Downloads + +2. download [primesense-nite2-nonfree_2.0.0-3_amd64.deb.gz](https://github.com/ros-drivers/openni2_tracker/files/287720/primesense-nite2-nonfree_2.0.0-3_amd64.deb.gz), `gunzip` it and run `dpkg-i` + +3. run `dpkg -i /var/cache/primesense-nite2-nonfree/openni-module-primesense-nite2-nonfree_2.0.0.0-1_amd64.deb` + +# How to run demo + +1. To run openni2 driver and openni2 tracker at once + +`$ roslsaunch openni2_tracker openni2_tracker.launch` will start + +2. To run openni2 driver and openni2 tracker separately + +`$ roslsaunch openni2_tracker openni2_launch openni2.launch` + +`$ ROS_NAMESPACE=camera roslaunch openni2_tracker_standalone.launch manager:=camera_nodelet_manager is_standalone:=false` + + + +# Maintainer tips + +https://github.com/ros-drivers/openni2_tracker/issues/4#issuecomment-221925076 + +https://github.com/k-okada/primesense-nite2-nonfree-2.0/blob/master/update-primesense-nite2-nonfree + +https://github.com/k-okada/primesense-nite2-nonfree-2.0/blob/master/primesense-nite2-nonfree-make-deb diff --git a/include/openni2_tracker/openni2_tracker.h b/include/openni2_tracker/openni2_tracker.h new file mode 100644 index 0000000..2b221fe --- /dev/null +++ b/include/openni2_tracker/openni2_tracker.h @@ -0,0 +1,59 @@ +#ifndef OPENNI2_TRACKER_NODELET_CLASS_H_ +#define OPENNI2_TRACKER_NODELET_CLASS_H_ + +#include +#include +#include +#include +#include + +#include +#include + +namespace openni2_tracker { + +class OpenNI2TrackerNodelet : public nodelet::Nodelet { +public: + OpenNI2TrackerNodelet(); + ~OpenNI2TrackerNodelet(); + + void onInit(); + void publishTransform(nite::UserData const &user, + nite::JointType const &joint, + std::string const &frame_id, + std::string const &child_frame_id); + void imageCallback(const sensor_msgs::ImageConstPtr& msg); + void timeCallback(const ros::TimerEvent); + void publishTransforms(const std::string &frame_id, + const nite::Array &users); + + void updateUserState(const nite::UserData &user, + unsigned long long ts); + + void device_initialization(); + +private: + boost::shared_ptr nh_; + boost::shared_ptr pnh_; + boost::shared_ptr it_; + image_transport::Subscriber depth_img_sub_; + ros::Timer publish_timer_; + tf::TransformBroadcaster broadcaster_; + + boost::mutex mutex_; + boost::shared_ptr userTrackerFrame_; + boost::shared_ptr userTracker_; + nite::Status niteRc_; + std::vector g_visibleUsers; + std::vector g_skeletonStates; + openni::Device devDevice_; + + std::string frame_id_; + std::string device_id_; + double publish_period_; + int max_users_; +}; + +} // namespace openni2_tracker + +#endif /* OPENNI2_TRACKER_NODELET_CLASS_H_ */ diff --git a/launch/openni2_tracker.launch b/launch/openni2_tracker.launch new file mode 100644 index 0000000..a493ced --- /dev/null +++ b/launch/openni2_tracker.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/launch/openni2_tracker_standalone.launch b/launch/openni2_tracker_standalone.launch new file mode 100644 index 0000000..53048a2 --- /dev/null +++ b/launch/openni2_tracker_standalone.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + diff --git a/openni2_tracker_nodelet.xml b/openni2_tracker_nodelet.xml new file mode 100644 index 0000000..e57efca --- /dev/null +++ b/openni2_tracker_nodelet.xml @@ -0,0 +1,7 @@ + + + + OpenNI2Tracker. + + + diff --git a/package.xml b/package.xml index a00c165..cd57e34 100644 --- a/package.xml +++ b/package.xml @@ -12,23 +12,29 @@ catkin + image_transport libopenni2-dev libusb-1.0-dev libopenni-sensor-primesense-dev --> geometry_msgs - orocos_kdl + nodelet roscpp roslib tf + image_transport libopenni2-dev libusb-1.0-dev libopenni-sensor-primesense-dev geometry_msgs - orocos_kdl + nodelet roscpp roslib tf + + + + diff --git a/src/openni2_tracker.cpp b/src/openni2_tracker.cpp deleted file mode 100644 index 71ca235..0000000 --- a/src/openni2_tracker.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/* -* Copyright (c) 2013, Marcus Liebhardt, Yujin Robot. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* * Neither the name of Yujin Robot nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*/ - -/* - * Inspired by the openni_tracker by Tim Field and PrimeSense's NiTE 2.0 - Simple Skeleton Sample - */ - -#include -#include -#include -#include - -//#include -//#include -//#include -#include - -using std::string; - -//xn::Context g_Context; -//xn::DepthGenerator g_DepthGenerator; -//xn::UserGenerator g_UserGenerator; - -#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);} - -//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; \ -// } - -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) { -// 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"); - - 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 frame_id("openni_depth_frame"); - nh_priv.getParam("camera_frame_id", frame_id); - - 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& 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()); -// 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); - } - } -// publishTransforms(frame_id); - } - else - { - ROS_WARN_STREAM("Get next frame failed."); - } - - r.sleep(); - } - return 0; -} diff --git a/src/openni2_tracker_nodelet.cpp b/src/openni2_tracker_nodelet.cpp new file mode 100644 index 0000000..de9caf1 --- /dev/null +++ b/src/openni2_tracker_nodelet.cpp @@ -0,0 +1,264 @@ +/* +* Copyright (c) 2013, Marcus Liebhardt, Yujin Robot. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* * Neither the name of Yujin Robot nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*/ + +/* + * Inspired by the openni_tracker by Tim Field and PrimeSense's NiTE 2.0 - Simple Skeleton Sample + */ +#include +#include +#include +#include +#include + +//#include +//#include +//#include +#include + +#include + +PLUGINLIB_EXPORT_CLASS(openni2_tracker::OpenNI2TrackerNodelet, nodelet::Nodelet) + +namespace openni2_tracker { + +OpenNI2TrackerNodelet::OpenNI2TrackerNodelet() : max_users_(10) {} + +OpenNI2TrackerNodelet::~OpenNI2TrackerNodelet() { + NODELET_INFO("shutdown: device %s", device_id_.c_str()); + userTrackerFrame_->release(); + userTracker_->destroy(); + devDevice_.close(); + nite::NiTE::shutdown(); +} + +using std::string; + +#define USER_MESSAGE(msg) \ + {printf("[%08llu] User #%d:\t%s\n",ts, user.getId(),msg);} + +void OpenNI2TrackerNodelet::onInit() { + + for (int i = 0; i < max_users_; ++i) { + g_visibleUsers.push_back(false); + g_skeletonStates.push_back(nite::SKELETON_NONE); + } + + nh_.reset(new ros::NodeHandle(getMTNodeHandle())); + pnh_.reset(new ros::NodeHandle(getMTPrivateNodeHandle())); + it_ = boost::shared_ptr(new image_transport::ImageTransport(*nh_)); + + frame_id_ = "openni_depth_frame"; + pnh_->getParam("camera_frame_id", frame_id_); + if (!pnh_->getParam("device_id", device_id_)) { + device_id_ = "#1"; + } + NODELET_INFO("device_id: %s", device_id_.c_str()); + + nh_->param("publish_period", publish_period_, 33); + publish_period_ = publish_period_ / 1000.0; + publish_timer_ = nh_->createTimer( + ros::Duration(publish_period_), + boost::bind(&OpenNI2TrackerNodelet::timeCallback, this, _1)); + + bool is_standalone; + if (pnh_->getParam("is_standalone", is_standalone) && is_standalone) { + device_initialization(); + } else { + NODELET_INFO("waiting for %s to initialize OpenNI", nh_->resolveName("image").c_str()); + depth_img_sub_ = it_->subscribe("image", 1, &OpenNI2TrackerNodelet::imageCallback, this); + } + +} + +void OpenNI2TrackerNodelet::device_initialization() { + boost::mutex::scoped_lock lock(mutex_); + if (openni::OpenNI::initialize() != openni::STATUS_OK) { + NODELET_FATAL("OpenNI initial error"); + return; + } + + openni::Array deviceInfoList; + openni::OpenNI::enumerateDevices(&deviceInfoList); + + if (device_id_.size() == 0 || device_id_ == "#1") { + device_id_ = deviceInfoList[0].getUri(); + } + + if (devDevice_.open(device_id_.c_str()) != openni::STATUS_OK) { + NODELET_FATAL("Couldn't open device: %s", device_id_.c_str()); + return; + } + + userTrackerFrame_.reset(new nite::UserTrackerFrameRef); + userTracker_.reset(new nite::UserTracker); + nite::NiTE::initialize(); + + niteRc_ = userTracker_->create(&devDevice_); + if (niteRc_ != nite::STATUS_OK) { + NODELET_FATAL("Couldn't create user tracker"); + return; + } + NODELET_INFO("OpenNI userTracker initialized"); +} + +void OpenNI2TrackerNodelet::imageCallback(const sensor_msgs::ImageConstPtr& msg) +{ + depth_img_sub_.shutdown(); + device_initialization(); +} + +void OpenNI2TrackerNodelet::timeCallback(const ros::TimerEvent) { + boost::mutex::scoped_lock lock(mutex_); + if (!devDevice_.isValid()) + return; + niteRc_ = userTracker_->readFrame(&(*userTrackerFrame_)); + if (niteRc_ != nite::STATUS_OK) { + NODELET_WARN("Get next frame failed."); + return; + } + + const nite::Array &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()); + NODELET_INFO("Found a new user."); + } else if (user.getSkeleton().getState() == nite::SKELETON_TRACKED) { + NODELET_INFO("Now tracking user %d", user.getId()); + } + } + publishTransforms(frame_id_, users); +} + +void OpenNI2TrackerNodelet::publishTransform(nite::UserData const& user, nite::JointType const& joint, string const& frame_id, string const& child_frame_id) { + static tf::TransformBroadcaster br; + + nite::SkeletonJoint joint_position = user.getSkeleton().getJoint(joint); + double x = joint_position.getPosition().x / 1000.0; + double y = joint_position.getPosition().z / 1000.0; + double z = joint_position.getPosition().y / 1000.0; + + double qx = joint_position.getOrientation().x; + double qy = joint_position.getOrientation().z; + double qz = -joint_position.getOrientation().y; + double qw = joint_position.getOrientation().w; + + char child_frame_no[128]; + snprintf(child_frame_no, sizeof(child_frame_no), "%s_%d", child_frame_id.c_str(), user.getId()); + + if (qx == 0 && qy == 0 && qz == 0) { + printf("%s has no Orientation: %f %f %f %f. Using default.\n", child_frame_no, qx, qy, qz, qw); + qw = 1.0; + } + + tf::Transform transform; + transform.setOrigin(tf::Vector3(x, y, z)); + transform.setRotation(tf::Quaternion(qx, -qy, -qz, qw)); + + tf::Transform change_frame; + change_frame.setOrigin(tf::Vector3(0, 0, 0)); + tf::Quaternion frame_rotation; + frame_rotation.setEulerZYX(1.5708, 3.1416, 3.1416); + change_frame.setRotation(frame_rotation); + + transform = change_frame * transform; + + br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_no)); +} + + +void OpenNI2TrackerNodelet::publishTransforms(const std::string& frame_id, const nite::Array& users) { + for (int i = 0; i < users.getSize(); ++i) { + const nite::UserData& user = users[i]; + + if (user.getSkeleton().getState() != nite::SKELETON_TRACKED) + continue; + + publishTransform(user, nite::JOINT_HEAD, frame_id, "head"); + publishTransform(user, nite::JOINT_NECK, frame_id, "neck"); + publishTransform(user, nite::JOINT_TORSO, frame_id, "torso"); + + publishTransform(user, nite::JOINT_LEFT_SHOULDER, frame_id, "left_shoulder"); + publishTransform(user, nite::JOINT_LEFT_ELBOW, frame_id, "left_elbow"); + publishTransform(user, nite::JOINT_LEFT_HAND, frame_id, "left_hand"); + + publishTransform(user, nite::JOINT_RIGHT_SHOULDER, frame_id, "right_shoulder"); + publishTransform(user, nite::JOINT_RIGHT_ELBOW, frame_id, "right_elbow"); + publishTransform(user, nite::JOINT_RIGHT_HAND, frame_id, "right_hand"); + + publishTransform(user, nite::JOINT_LEFT_HIP, frame_id, "left_hip"); + publishTransform(user, nite::JOINT_LEFT_KNEE, frame_id, "left_knee"); + publishTransform(user, nite::JOINT_LEFT_FOOT, frame_id, "left_foot"); + + publishTransform(user, nite::JOINT_RIGHT_HIP, frame_id, "right_hip"); + publishTransform(user, nite::JOINT_RIGHT_KNEE, frame_id, "right_knee"); + publishTransform(user, nite::JOINT_RIGHT_FOOT, frame_id, "right_foot"); + } +} + +void OpenNI2TrackerNodelet::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; + } + } +} + +} // end of openni2_tracker namespace