[jsk_perception][skeleton_with_depth] add nodelet version

jsk_perception/CMakeLists.txt

@@ -290,6 +290,7 @@ jsk_add_nodelet(src/gaussian_blur.cpp "jsk_perception/GaussianBlur" "gaussian_bl
 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/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/bing.cpp "jsk_perception/Bing" "bing")
 endif()

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 <[email protected]>
+ */
+
+#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/plugins/nodelet/libjsk_perception.xml

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

jsk_perception/src/skeleton_with_depth.cpp

@@ -0,0 +1,175 @@
+// -*- 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.cpp
+ * Author: Yoshiki Obinata <[email protected]>
+ */
+
+#include <jsk_perception/skeleton_with_depth.h>
+
+namespace jsk_perception{
+
+    void SkeletonWithDepth::onInit(){
+        DiagnosticNodelet::onInit();
+        pub_skeleton_ = advertise<jsk_recognition_msgs::HumanSkeletonArray>(*pnh_, "output/skeleton", 1);
+        pnh_->param("approximate_sync", approximate_sync_, true);
+        onInitPostProcess();
+    }
+
+    SkeletonWithDepth::SkeletonWithDepth::~SkeletonWithDepth(){
+        if(approximate_sync_){
+            ap_sync_.reset();
+        }else{
+            sync_.reset();
+        }
+    }
+
+    void SkeletonWithDepth::subscribe(){
+        sub_skeleton_.subscribe(*pnh_, "input/skeleton", 1);
+        sub_depth_.subscribe(*pnh_, "input/depth", 1);
+        sub_cam_info_ = pnh_->subscribe("input/info", 1, &SkeletonWithDepth::camInfoCallback, this);
+        if(approximate_sync_){
+            ap_sync_ = std::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(100);
+            ap_sync_->connectInput(sub_skeleton_, sub_depth_);
+            ap_sync_->registerCallback(boost::bind(&SkeletonWithDepth::callback,
+                                                   this,
+                                                   boost::placeholders::_1,
+                                                   boost::placeholders::_2));
+        }else{
+            sync_ = std::make_shared<message_filters::Synchronizer<ExactSyncPolicy> >(100);
+            sync_->connectInput(sub_skeleton_, sub_depth_);
+            sync_->registerCallback(boost::bind(&SkeletonWithDepth::callback,
+                                                this,
+                                                boost::placeholders::_1,
+                                                boost::placeholders::_2));
+        }
+    }
+
+    void SkeletonWithDepth::unsubscribe(){
+        sub_skeleton_.unsubscribe();
+        sub_depth_.unsubscribe();
+        sub_cam_info_.shutdown();
+    }
+
+    void SkeletonWithDepth::camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg){
+        cam_info_ = *msg;
+    }
+
+    void SkeletonWithDepth::callback(const jsk_recognition_msgs::HumanSkeletonArray::ConstPtr& skeleton_array_msg,
+                                     const sensor_msgs::Image::ConstPtr& depth_msg){
+        std::lock_guard<std::mutex> lock(mutex_);
+        // check camera info
+        if(cam_info_ == sensor_msgs::CameraInfo()){
+            NODELET_ERROR("Camera info is not set");
+            return;
+        }
+        image_geometry::PinholeCameraModel cam_model_;
+        cam_model_.fromCameraInfo(cam_info_);
+
+        // convert depth image
+        cv_bridge::CvImagePtr cv_depth_ptr;
+        try{
+            cv_depth_ptr = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_32FC1);
+        }catch(cv_bridge::Exception& e){
+            NODELET_ERROR("cv_bridge exception: %s", e.what());
+            return;
+        }
+        if(cv_depth_ptr->image.type() == CV_16UC1){
+            cv::Mat tmp;
+            cv_depth_ptr->image.convertTo(tmp, CV_32FC1, 0.001);
+            cv_depth_ptr->image = tmp;
+        }else if(cv_depth_ptr->image.type() != CV_32FC1){
+            NODELET_ERROR("Unsupported depth image type: %d", cv_depth_ptr->image.type());
+            return;
+        }
+        const cv::Mat depth = cv_depth_ptr->image;
+
+        // convert skeleton
+        jsk_recognition_msgs::HumanSkeletonArray skeleton_array_output;
+        skeleton_array_output.header = skeleton_array_msg->header;
+        const int H = depth.rows;
+        const int W = depth.cols;
+        for(auto skeleton : skeleton_array_msg->skeletons){
+            jsk_recognition_msgs::HumanSkeleton skeleton_output;
+            skeleton_output.header = skeleton_array_msg->header;
+            for(auto bone_info : boost::combine(skeleton.bone_names, skeleton.bones)){
+                std::string bone_name;
+                jsk_recognition_msgs::Segment bone;
+                boost::tie(bone_name, bone) = bone_info;
+
+                // bone start point
+                if ((0 <= bone.start_point.x < W) && (0 <= bone.start_point.y < H)){
+                    const float z_start = depth.at<float>((int)bone.start_point.y, (int)bone.start_point.x);
+                    bone.start_point.z = z_start;
+                    bone.start_point.x = (bone.start_point.x - cam_model_.cx()) * z_start / cam_model_.fx();
+                    bone.start_point.y = (bone.start_point.y - cam_model_.cy()) * z_start / cam_model_.fy();
+                    if (bone.start_point.z <= 0){
+                        continue;
+                    }
+                }else{
+                    continue;
+                }
+
+                // bone end point
+                if ((0 <= bone.end_point.x < W) && (0 <= bone.end_point.y < H)){
+                    const float z_end = depth.at<float>((int)bone.end_point.y, (int)bone.end_point.x);
+                    bone.end_point.z = z_end;
+                    bone.end_point.x = (bone.end_point.x - cam_model_.cx()) * z_end / cam_model_.fx();
+                    bone.end_point.y = (bone.end_point.y - cam_model_.cy()) * z_end / cam_model_.fy();
+                    if (bone.start_point.z <= 0){
+                        continue;
+                    }
+                }else{
+                    continue;
+                }
+                skeleton_output.bone_names.push_back(bone_name);
+                skeleton_output.bones.push_back(bone);
+            }
+            skeleton_array_output.skeletons.push_back(skeleton_output);
+        }
+        vital_checker_->poke();
+        pub_skeleton_.publish(skeleton_array_output);
+    }
+
+    void SkeletonWithDepth::updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat){
+        if(vital_checker_ -> isAlive()){
+            stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "SkeletonWithDepth running\n");
+        }
+        DiagnosticNodelet::updateDiagnostic(stat);
+    }
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS (jsk_perception::SkeletonWithDepth, nodelet::Nodelet);