Merge pull request IntelRealSense#21 from sgutlian/dev

Add ROS enum for skeleton joints

realsense_ros_person/README.md

@@ -14,10 +14,20 @@ The person package consists of 2 nodelets:
 2. realsense_ros_person_sample_nodelet - demo for usage of Person ROS API - visualize output of realsense_ros_person nodelet, provide GUI for realsense_ros_person node control(Start tracking, Register/Recognize users)
 
 ## 2. Run person nodes (Person API wrapper + sample):
+Person node starts in detection mode, some features requries tracking mode(e.g. skeleton).
+
+Aditional information:
+
+* [Person features tracking mode requirements](#4-person-features-requirements-for-tracking-mode)
+
+* [Person sample nodelet GUI commands](#324-gui-commands)
+
 ### For person tracking feature
     roslaunch realsense_ros_person realsense_person_demo_tracking.launch
 ### For person gestures feature(pointing & wave)
     roslaunch realsense_ros_person realsense_person_demo_gestures.launch
+### For skeleton feature
+    roslaunch realsense_ros_person realsense_person_demo_skeleton.launch
 ### For person face features (recogntion, landmarks, head pose, head bounding box)
     roslaunch realsense_ros_person realsense_person_demo_face_features.launch
 
@@ -82,7 +92,7 @@ The person package consists of 2 nodelets:
     nothing
 #### 3.2.3 Services
     nothing
-####  3.2.4 GUI commands:
+####  3.2.4 GUI commands
     Start tracking:
         Mouse wheel click
     Register:
@@ -109,3 +119,17 @@ configuration request will be send to realsense_ros_person nodelet
         Enable/Disable head bounding box
     ~landmarksEnabled (bool, default: false)
         Enable/Disable face landmarks
+
+# 4 Person features requirements for tracking mode
+Part of person features works only at tracking/detection mode.
+
+|Feature name           |Tracking mode          |
+|-----------------------|-----------------------|
+|Tracking               |Tracking               |
+|Skeleton               |Tracking               |
+|Pointing gesture       |Tracking               |
+|Wave gesture           |Detection              |
+|Recognition            |Not important          |
+|Landmarks              |Not important          |
+|Head bounding box      |Not important          |
+|Head pose              |Not important          |

realsense_ros_person/launch/realsense_person_demo_skeleton.launch

@@ -0,0 +1,24 @@
+<launch>
+
+  <include file="$(find realsense_ros_camera)/launch/camera.launch">
+    <arg name="depth_width" value="320"/>
+    <arg name="depth_height" value="240"/>
+    <arg name="color_width" value="640"/>
+    <arg name="color_height" value="480"/>
+  </include>
+
+  <include file="$(find realsense_ros_person)/launch/person_tracking.launch"/>
+
+    <node pkg="nodelet" type="nodelet" name="realsense_ros_person_sample_nodelet"  args="manager" output="screen"/>
+        <node pkg="realsense_ros_person" type="realsense_ros_person_sample" name="realsense_ros_person_sample"
+            args="load realsense_ros_person/realsense_ros_person_sample node" output="screen">
+            <param name="recognitionEnabled"      type="bool"  value="false" />
+            <param name="skeletonEnabled"         type="bool"  value="true" />
+            <param name="pointingGestureEnabled"  type="bool"  value="false" />
+            <param name="waveGestureEnabled"      type="bool"  value="false" />
+            <param name="landmarksEnabled"        type="bool"  value="false" />
+            <param name="headBoundingBoxEnabled"  type="bool"  value="false" />
+            <param name="headPoseEnabled"         type="bool"  value="false" />
+        </node>
+
+</launch> 

realsense_ros_person/msg/SkeletonJoint.msg

@@ -2,3 +2,31 @@ int32 type
 float32 confidence
 geometry_msgs/Point location
 geometry_msgs/Point32 realWorldCoordinates
+
+#joint types constants
+int32 JOINT_ANKLE_LEFT=0
+int32 JOINT_ANKLE_RIGHT=1
+int32 JOINT_ELBOW_LEFT=2
+int32 JOINT_ELBOW_RIGHT=3
+int32 JOINT_FOOT_LEFT=4
+int32 JOINT_FOOT_RIGHT=5
+int32 JOINT_HAND_LEFT=6
+int32 JOINT_HAND_RIGHT=7
+int32 JOINT_HAND_TIP_LEFT=8
+int32 JOINT_HAND_TIP_RIGHT=9
+int32 JOINT_HEAD=10
+int32 JOINT_HIP_LEFT=11
+int32 JOINT_HIP_RIGHT=12
+int32 JOINT_KNEE_LEFT=13
+int32 JOINT_KNEE_RIGHT=14
+int32 JOINT_NECK=15
+int32 JOINT_SHOULDER_LEFT=16
+int32 JOINT_SHOULDER_RIGHT=17
+int32 JOINT_SPINE_BASE=18
+int32 JOINT_SPINE_MID=19
+int32 JOINT_SPINE_SHOULDER=20
+int32 JOINT_THUMB_LEFT=21
+int32 JOINT_THUMB_RIGHT=22
+int32 JOINT_WRIST_LEFT=23
+int32 JOINT_WRIST_RIGHT=24
+int32 JOINT_UNKNOWN=25

realsense_ros_person/src/apiwrapper/helpers/PersonTracking2RosHelper.h

@@ -9,6 +9,7 @@
 #include "realsense_ros_person/RecognitionRegisterResponse.h"
 #include "realsense_ros_person/Wave.h"
 #include "realsense_ros_person/RectWithConfidence.h"
+#include "realsense_ros_person/SkeletonJoint.h"
 
 namespace realsense_ros_person
 {
@@ -98,5 +99,64 @@ namespace realsense_ros_person
             rectWithConfidence.confidence = boundingBox2D.confidence;
             return rectWithConfidence;
         }
+
+        int SkeletonJointType2RosSkeletonJointType(Intel::RealSense::PersonTracking::PersonJoints::JointType jointType)
+        {
+            using namespace Intel::RealSense::PersonTracking;
+            switch (jointType) {
+                case PersonJoints::JointType::JOINT_ANKLE_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_ANKLE_LEFT;
+                case PersonJoints::JointType::JOINT_ANKLE_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_ANKLE_RIGHT;
+                case PersonJoints::JointType::JOINT_ELBOW_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_ELBOW_LEFT;
+                case PersonJoints::JointType::JOINT_ELBOW_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_ELBOW_RIGHT;
+                case PersonJoints::JointType::JOINT_FOOT_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_FOOT_LEFT;
+                case PersonJoints::JointType::JOINT_FOOT_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_FOOT_RIGHT;
+                case PersonJoints::JointType::JOINT_HAND_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_HAND_LEFT;
+                case PersonJoints::JointType::JOINT_HAND_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_HAND_RIGHT;
+                case PersonJoints::JointType::JOINT_HAND_TIP_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_HAND_TIP_LEFT;
+                case PersonJoints::JointType::JOINT_HAND_TIP_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_HAND_TIP_RIGHT;
+                case PersonJoints::JointType::JOINT_HEAD:
+                    return realsense_ros_person::SkeletonJoint::JOINT_HEAD;
+                case PersonJoints::JointType::JOINT_HIP_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_HIP_LEFT;
+                case PersonJoints::JointType::JOINT_HIP_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_HIP_RIGHT;
+                case PersonJoints::JointType::JOINT_KNEE_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_KNEE_LEFT;
+                case PersonJoints::JointType::JOINT_KNEE_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_KNEE_RIGHT;
+                case PersonJoints::JointType::JOINT_NECK:
+                    return realsense_ros_person::SkeletonJoint::JOINT_NECK;
+                case PersonJoints::JointType::JOINT_SHOULDER_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_SHOULDER_LEFT;
+                case PersonJoints::JointType::JOINT_SHOULDER_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_SHOULDER_RIGHT;
+                case PersonJoints::JointType::JOINT_SPINE_BASE:
+                    return realsense_ros_person::SkeletonJoint::JOINT_SPINE_BASE;
+                case PersonJoints::JointType::JOINT_SPINE_MID:
+                    return realsense_ros_person::SkeletonJoint::JOINT_SPINE_MID;
+                case PersonJoints::JointType::JOINT_SPINE_SHOULDER:
+                    return realsense_ros_person::SkeletonJoint::JOINT_SPINE_SHOULDER;
+                case PersonJoints::JointType::JOINT_THUMB_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_THUMB_LEFT;
+                case PersonJoints::JointType::JOINT_THUMB_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_THUMB_RIGHT;
+                case PersonJoints::JointType::JOINT_WRIST_LEFT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_WRIST_LEFT;
+                case PersonJoints::JointType::JOINT_WRIST_RIGHT:
+                    return realsense_ros_person::SkeletonJoint::JOINT_WRIST_RIGHT;
+                default:
+                    return realsense_ros_person::SkeletonJoint::JOINT_UNKNOWN;
+            }
+        }
     };
 }

realsense_ros_person/src/apiwrapper/publisher/PersonTrackingPublisherHelper.cpp

@@ -4,7 +4,7 @@
 #include <ros/console.h>
 
 #include "PersonTrackingPublisherHelper.h"
-#include "RealSense/Image.h"
+#include "realsense_ros_person/SkeletonJoint.h"
 #include "PersonTrackingHelper.h"
 
 
@@ -32,7 +32,7 @@ namespace realsense_ros_person
         for (PersonTrackingData::PersonJoints::SkeletonPoint &skeletonPoint : skeletonPoints)
         {
             realsense_ros_person::SkeletonJoint joint;
-            joint.type = skeletonPoint.jointType;
+            joint.type = m_pt2rosHelper.SkeletonJointType2RosSkeletonJointType(skeletonPoint.jointType);
             joint.confidence = skeletonPoint.confidenceImage;
             joint.location.x = skeletonPoint.image.x;
             joint.location.y = skeletonPoint.image.y;