rewrited version of #3 (continued porting to NiTe 2 (Status: all tf sending))

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)

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

include/openni2_tracker/openni2_tracker.h

@@ -0,0 +1,59 @@
+#ifndef OPENNI2_TRACKER_NODELET_CLASS_H_
+#define OPENNI2_TRACKER_NODELET_CLASS_H_
+
+#include <nodelet/nodelet.h>
+#include <ros/package.h>
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <image_transport/image_transport.h>
+
+#include <NiTE.h>
+#include <OpenNI.h>
+
+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<nite::UserData> &users);
+
+  void updateUserState(const nite::UserData &user,
+                       unsigned long long ts);
+
+  void device_initialization();
+
+private:
+  boost::shared_ptr<ros::NodeHandle> nh_;
+  boost::shared_ptr<ros::NodeHandle> pnh_;
+  boost::shared_ptr<image_transport::ImageTransport> it_;
+  image_transport::Subscriber depth_img_sub_;
+  ros::Timer publish_timer_;
+  tf::TransformBroadcaster broadcaster_;
+
+  boost::mutex mutex_;
+  boost::shared_ptr<nite::UserTrackerFrameRef> userTrackerFrame_;
+  boost::shared_ptr<nite::UserTracker> userTracker_;
+  nite::Status niteRc_;
+  std::vector<bool> g_visibleUsers;
+  std::vector<nite::SkeletonState> 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_ */

launch/openni2_tracker.launch

@@ -0,0 +1,19 @@
+<launch>
+
+  <arg name="camera" default="camera" />
+  <arg name="manager" value="$(arg camera)_nodelet_manager" />
+
+  <include file="$(find openni2_launch)/launch/openni2.launch" >
+    <arg name="camera" value="$(arg camera)" />
+  </include>
+
+  <group ns="$(arg camera)" >
+    <include file="$(find openni2_tracker)/launch/openni2_tracker_standalone.launch" >
+      <arg name="manager" value="$(arg manager)" />
+      <arg name="depth_image"   value="depth/image_raw" />
+      <arg name="camera_frame_id" value="camera_link" />
+      <arg name="is_standalone"   value="false" /> <!-- to share nodelet manager launched at openni2.launch -->
+    </include>
+  </group>
+
+</launch>

launch/openni2_tracker_standalone.launch

@@ -0,0 +1,23 @@
+<launch>
+
+  <arg name="manager"   default="manager"
+       doc="manager name"/>
+  <arg name="camera_frame_id" default="camera_link"
+       doc="camera farame_id for output" />
+  <arg name="is_standalone"   default="true"
+       doc="set true is you want to start standalone manager" />
+  <arg name="depth_image"   default="depth/image_raw"
+       doc="image topic to start tracker node, only for is_standalone=false"/>
+
+  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager"
+        output="screen" if="$(arg is_standalone)" />
+  <node name="openni2_tracker_nodelet"
+        pkg="nodelet" type="nodelet"
+        args="load openni2_tracker/OpenNI2TrackerNodelet $(arg manager)"
+        output="screen">
+    <remap from="image" to="$(arg depth_image)" />
+    <param name="is_standalone" value="$(arg is_standalone)" />
+    <param name="camera_frame_id" value="$(arg camera_frame_id)" />
+  </node>
+
+</launch>

openni2_tracker_nodelet.xml

@@ -0,0 +1,7 @@
+<library path="lib/libopenni2_tracker">
+  <class name="openni2_tracker/OpenNI2TrackerNodelet" type="openni2_tracker::OpenNI2TrackerNodelet" base_class_type="nodelet::Nodelet">
+    <description>
+      OpenNI2Tracker.
+    </description>
+  </class>
+</library>

package.xml

@@ -12,23 +12,29 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>image_transport</build_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>nodelet</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>roslib</build_depend>
   <build_depend>tf</build_depend>
 
+  <run_depend>image_transport</run_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>nodelet</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>roslib</run_depend>
   <run_depend>tf</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/openni2_tracker_nodelet.xml" />
+  </export>
 </package>