Add nodelet version of Skeleton with depth

jsk_perception/CMakeLists.txt

@@ -293,6 +293,7 @@ jsk_add_nodelet(src/kmeans.cpp "jsk_perception/KMeans" "kmeans")
 jsk_add_nodelet(src/draw_rects.cpp "jsk_perception/DrawRects" "draw_rects")
 jsk_add_nodelet(src/remove_blurred_frames.cpp "jsk_perception/RemoveBlurredFrames" "remove_blurred_frames")
 jsk_add_nodelet(src/split_image.cpp "jsk_perception/SplitImage" "split_image")
+jsk_add_nodelet(src/skeleton_with_depth.cpp "jsk_perception/SkeletonWithDepth" "skeleton_with_depth")
 if("${OpenCV_VERSION}" VERSION_GREATER "2.9.9")  # >= 3.0.0
   jsk_add_nodelet(src/video_to_scene.cpp "jsk_perception/VideoToScene" "video_to_scene")
   jsk_add_nodelet(src/bing.cpp "jsk_perception/Bing" "bing")

jsk_perception/include/jsk_perception/skeleton_with_depth.h

@@ -0,0 +1,94 @@
+// -*- mode: C++ -*-
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, JSK Lab
+ *  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 the JSK Lab 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.
+ *********************************************************************/
+/*
+ * skeleton_with_depth.h
+ * Author: Yoshiki Obinata <obinata@jsk.imi.i.u-tokyo.ac.jp>
+ */
+
+#ifndef SKELETON_WITH_DEPTH_H_
+#define SKELETON_WITH_DEPTH_H_
+
+#include <boost/range/combine.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <image_geometry/pinhole_camera_model.h>
+#include <jsk_recognition_msgs/HumanSkeleton.h>
+#include <jsk_recognition_msgs/HumanSkeletonArray.h>
+#include <jsk_topic_tools/diagnostic_nodelet.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <mutex>
+#include <sensor_msgs/Image.h>
+
+
+namespace jsk_perception{
+
+    class SkeletonWithDepth : public jsk_topic_tools::DiagnosticNodelet{
+    public:
+    SkeletonWithDepth() : DiagnosticNodelet("SkeletonWithDepth"){}
+        virtual ~SkeletonWithDepth();
+
+    protected:
+        virtual void onInit();
+        virtual void subscribe();
+        virtual void unsubscribe();
+        virtual void callback(const jsk_recognition_msgs::HumanSkeletonArray::ConstPtr& skeleton_msg,
+                              const sensor_msgs::Image::ConstPtr& depth_msg);
+        virtual void camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& cam_info_msg);
+        virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat);
+
+        message_filters::Subscriber<jsk_recognition_msgs::HumanSkeletonArray> sub_skeleton_;
+        message_filters::Subscriber<sensor_msgs::Image> sub_depth_;
+        ros::Subscriber sub_cam_info_; // not synchronized
+        ros::Publisher pub_skeleton_;
+
+        sensor_msgs::CameraInfo cam_info_ = sensor_msgs::CameraInfo();
+
+        typedef message_filters::sync_policies::ExactTime<jsk_recognition_msgs::HumanSkeletonArray, sensor_msgs::Image> ExactSyncPolicy;
+        typedef message_filters::sync_policies::ApproximateTime<jsk_recognition_msgs::HumanSkeletonArray, sensor_msgs::Image> ApproximateSyncPolicy;
+        std::shared_ptr<message_filters::Synchronizer<ExactSyncPolicy> > sync_;
+        std::shared_ptr<message_filters::Synchronizer<ApproximateSyncPolicy> > ap_sync_;
+
+        std::mutex mutex_;
+
+        bool approximate_sync_;
+
+    private:
+
+    };
+}
+
+#endif // SKELETON_WITH_DEPTH_H_

jsk_perception/node_scripts/skeleton_with_depth.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+# -*- coding:utf-8 -*-
+
+from __future__ import division
+from __future__ import print_function
+
+import cv_bridge
+from image_geometry import PinholeCameraModel
+from jsk_recognition_msgs.msg import HumanSkeleton
+from jsk_recognition_msgs.msg import HumanSkeletonArray
+from jsk_topic_tools import ConnectionBasedTransport
+import message_filters
+import numpy as np
+import rospy
+from sensor_msgs.msg import CameraInfo
+from sensor_msgs.msg import Image
+
+
+class SkeletonWithDepth(ConnectionBasedTransport):
+
+    def __init__(self):
+        super(self.__class__, self).__init__()
+        self.skeleton_pub = self.advertise(
+            '~output/skeleton', HumanSkeletonArray, queue_size=1)
+        self.bridge = cv_bridge.CvBridge()
+
+    def subscribe(self):
+        queue_size = rospy.get_param('~queue_size', 10)
+        sub_skeleton = message_filters.Subscriber(
+            '~input/skeleton',
+            HumanSkeletonArray,
+            queue_size=1, buff_size=2**24)
+        sub_depth = message_filters.Subscriber(
+            '~input/depth',
+            Image,
+            queue_size=1, buff_size=2**24)
+        self.subs = [sub_skeleton, sub_depth]
+
+        # NOTE: Camera info is not synchronized by default.
+        # See https://github.com/jsk-ros-pkg/jsk_recognition/issues/2165
+        sync_cam_info = rospy.get_param("~sync_camera_info", False)
+        if sync_cam_info:
+            sub_info = message_filters.Subscriber(
+                '~input/info',
+                CameraInfo, queue_size=1, buff_size=2**24)
+            self.subs.append(sub_info)
+        else:
+            self.sub_info = rospy.Subscriber(
+                '~input/info',
+                CameraInfo, self._cb_cam_info)
+
+        if rospy.get_param('~approximate_sync', True):
+            slop = rospy.get_param('~slop', 0.1)
+            sync = message_filters.ApproximateTimeSynchronizer(
+                fs=self.subs, queue_size=queue_size, slop=slop)
+        else:
+            sync = message_filters.TimeSynchronizer(
+                fs=self.subs, queue_size=queue_size)
+        if sync_cam_info:
+            sync.registerCallback(self._cb_with_depth_info)
+        else:
+            self.camera_info_msg = None
+            sync.registerCallback(self._cb_with_depth)
+
+    def unsubscribe(self):
+        for sub in self.subs:
+            sub.unregister()
+        if self.sub_info is not None:
+            self.sub_info.unregister()
+            self.sub_info = None
+
+    def _cb_cam_info(self, msg):
+        self.camera_info_msg = msg
+        self.sub_info.unregister()
+        self.sub_info = None
+        rospy.loginfo("Received camera info")
+
+    def _cb_with_depth(self, skeleton_msg, depth_msg):
+        if self.camera_info_msg is None:
+            return
+        self._cb_with_depth_info(skeleton_msg, depth_msg, self.camera_info_msg)
+
+    def _cb_with_depth_info(self, skeleton_msg, depth_msg, camera_info_msg):
+        camera_model = PinholeCameraModel()
+        camera_model.fromCameraInfo(camera_info_msg)
+        br = cv_bridge.CvBridge()
+        depth_img = br.imgmsg_to_cv2(depth_msg, 'passthrough')
+        if depth_msg.encoding == '16UC1':
+            depth_img = np.asarray(depth_img, dtype=np.float32)
+            depth_img /= 1000  # convert metric: mm -> m
+        elif depth_msg.encoding != '32FC1':
+            rospy.logerr('Unsupported depth encoding: %s' % depth_msg.encoding)
+
+        H, W = depth_img.shape
+        out_skeleton_array_msg = HumanSkeletonArray(header=skeleton_msg.header)
+        for skeleton in skeleton_msg.skeletons:
+            limb_to_pose = {}
+
+            out_skeleton_msg = HumanSkeleton(header=skeleton_msg.header)
+            for bone_name, bone in zip(skeleton.bone_names,
+                                       skeleton.bones):
+                u, v = bone.start_point.x, bone.start_point.y
+                if 0 <= u < W and 0 <= v < H:
+                    z = float(depth_img[int(v)][int(u)])
+                else:
+                    continue
+                if np.isnan(z) or z <= 0:
+                    continue
+                start_x = (u - camera_model.cx()) * z / camera_model.fx()
+                start_y = (v - camera_model.cy()) * z / camera_model.fy()
+                start_z = z
+
+                u, v = bone.end_point.x, bone.end_point.y
+                if 0 <= u < W and 0 <= v < H:
+                    z = float(depth_img[int(v)][int(u)])
+                else:
+                    continue
+                if np.isnan(z) or z <= 0:
+                    continue
+
+                a, b = bone_name.split('->')
+                limb_to_pose[a] = np.array(
+                    [bone.start_point.x,
+                     bone.start_point.y,
+                     bone.start_point.z])
+                limb_to_pose[b] = np.array(
+                    [bone.end_point.x,
+                     bone.end_point.y,
+                     bone.end_point.z])
+
+                bone.end_point.x = (u - camera_model.cx()
+                                    ) * z / camera_model.fx()
+                bone.end_point.y = (v - camera_model.cy()
+                                    ) * z / camera_model.fy()
+                bone.end_point.z = z
+
+                bone.start_point.x = start_x
+                bone.start_point.y = start_y
+                bone.start_point.z = start_z
+                out_skeleton_msg.bone_names.append(bone_name)
+                out_skeleton_msg.bones.append(bone)
+            out_skeleton_array_msg.skeletons.append(out_skeleton_msg)
+
+        self.skeleton_pub.publish(out_skeleton_array_msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node('skeleton_with_depth')
+    SkeletonWithDepth()
+    rospy.spin()

jsk_perception/plugins/nodelet/libjsk_perception.xml

@@ -306,4 +306,8 @@
          type="jsk_perception::SplitImage"
          base_class_type="nodelet::Nodelet">
   </class>
+  <class name="jsk_perception/SkeletonWithDepth"
+         type="jsk_perception::SkeletonWithDepth"
+         base_class_type="nodelet::Nodelet">
+  </class>
 </library>