[Fix] RGB data corrupt

CMakeLists.txt

@@ -26,11 +26,16 @@ add_message_files(FILES
     Appearance.msg
     Audio.msg    
     BodyArray.msg
+    Body.msg
+    BoundingBox2D.msg
     Expressions.msg
+    FaceArray.msg
+    Face.msg
+    FacePoints.msg
     JointOrientationAndType.msg
     JointPositionAndState.msg
     Lean.msg
-    Body.msg
+    Quaternion.msg
 )
 
 generate_messages(DEPENDENCIES
@@ -70,18 +75,22 @@ add_executable(startBody  "${PROJECT_SOURCE_DIR}/src/startBody.cpp")
 add_executable(startDepth "${PROJECT_SOURCE_DIR}/src/startDepth.cpp")
 add_executable(startIR    "${PROJECT_SOURCE_DIR}/src/startIR.cpp")
 add_executable(startRGB   "${PROJECT_SOURCE_DIR}/src/startRGB.cpp")
+add_executable(startFace  "${PROJECT_SOURCE_DIR}/src/startFace.cpp")
 
 add_dependencies(startAudio ${k2_client_EXPORTED_TARGETS})
 add_dependencies(startBody  ${k2_client_EXPORTED_TARGETS})
 add_dependencies(startDepth ${k2_client_EXPORTED_TARGETS})
 add_dependencies(startIR    ${k2_client_EXPORTED_TARGETS})
 add_dependencies(startRGB   ${k2_client_EXPORTED_TARGETS})
+add_dependencies(startFace  ${k2_client_EXPORTED_TARGETS})
 
 target_link_libraries(startAudio ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
 target_link_libraries(startBody  ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
 target_link_libraries(startDepth ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
 target_link_libraries(startIR    ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
 target_link_libraries(startRGB   ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
+target_link_libraries(startFace  ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
+
 
 
 

launch/k2_client.launch

@@ -1,14 +1,16 @@
 <launch>
-    <param name="serverNameOrIP" value="172.19.179.123" />
+    <include file="$(find tinker_machine)/tinker.machine"/>
+    <param name="serverNameOrIP" value="192.168.2.5" />
     <group ns="/head/kinect2">
         <param name="rgb_frame" value="/head/kinect2/rgb"/>
         <param name="depth_frame" value="/head/kinect2/depth"/>
         <param name="ir_frame" value="/head/kinect2/depth"/>
 
-        <node name="startRGB" pkg="k2_client" type="startRGB"/>
-        <node name="startDepth" pkg="k2_client" type="startDepth" />
-        <node name="startIR" pkg="k2_client" type="startIR"/>
-        <node name="startBody" pkg="k2_client" type="startBody"/>
-        <node name="startAudio" pkg="k2_client" type="startAudio"/>
-    </group>
+        <node machine="hyperv-linux" name="startRGB" pkg="k2_client" type="startRGB"/>
+        <node machine="hyperv-linux" name="startDepth" pkg="k2_client" type="startDepth" output="screen"/>
+        <node machine="hyperv-linux" name="startBody" pkg="k2_client" type="startBody" output="screen"/>
+        <node machine="hyperv-linux" name="startAudio" pkg="k2_client" type="startAudio" output="screen"/>
+        <!--node machine="hyperv-linux" name="startFace" pkg="k2_client" type="startFace" output="screen"/-->
+
+     </group>
 </launch>

msg/Body.msg

@@ -1,4 +1,8 @@
 Header header
+int32 fromX
+int32 toX
+int32 fromY
+int32 toY
 int32 leanTrackingState
 Lean lean
 bool isRestricted
@@ -14,4 +18,4 @@ Appearance appearance
 Activities activities
 Expressions expressions
 JointOrientationAndType[] jointOrientations
-JointPositionAndState[] jointPositions
+JointPositionAndState[] jointPositions

msg/BoundingBox2D.msg

@@ -0,0 +1,4 @@
+int32 left
+int32 top
+int32 right
+int32 bottom

msg/Face.msg

@@ -0,0 +1,16 @@
+Header header
+
+Appearance appearance
+Activities activities
+Expressions expressions
+
+FacePoints facePointsInInfraredSpace
+FacePoints facePointsInColorSpace
+
+BoundingBox2D faceBoundingBoxInInfraredSpace
+BoundingBox2D faceBoundingBoxInColorSpace
+
+Quaternion faceRotationQuaternion
+
+int64 trackingId
+int32 faceFrameFeatures

msg/FaceArray.msg

@@ -0,0 +1 @@
+Face[] faces

msg/FacePoints.msg

@@ -0,0 +1,10 @@
+float32 eyeLeftX
+float32 eyeLeftY
+float32 eyeRightX
+float32 eyeRightY
+float32 noseX
+float32 noseY
+float32 mouthCornerLeftX
+float32 mouthCornerLeftY
+float32 mouthCornerRightX
+float32 mouthCornerRightY

msg/Quaternion.msg

@@ -0,0 +1,4 @@
+float32 X
+float32 Y
+float32 Z
+float32 W

src/startAudio.cpp

@@ -26,7 +26,10 @@ CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ***************************************************************************************/
 #include "k2_client/k2_client.h"
-#include "k2_client/Audio.h"
+#include <std_msgs/Float64.h>
+#include <string>
+#include <ros/ros.h>
+
 
 std::string topicName = "audio";
 int twiceStreamSize = 8200;
@@ -38,33 +41,15 @@ int main(int argC,char **argV)
 	ros::NodeHandle n;
 	std::string serverAddress;
 	n.getParam("/serverNameOrIP",serverAddress);
-	Socket mySocket(serverAddress.c_str(),"9004",twiceStreamSize);
-	ros::Publisher audioPub = n.advertise<k2_client::Audio>(topicName,1);
+	Socket mySocket(serverAddress.c_str(),"9009",twiceStreamSize);
+	ros::Publisher audioPub = n.advertise<std_msgs::Float64>(topicName,1);
 	while(ros::ok())
 	{
 		mySocket.readData();
-		std::string jsonString;
-		for(int i=0;i<twiceStreamSize;i+=2)
-		{
-			jsonString += mySocket.mBuffer[i];
-		}
-		Json::Value jsonObject;
-		Json::Reader jsonReader;
-		bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
-		k2_client::Audio audio;
-		audio.header.stamp = ros::Time(jsonObject["utcTime"].asDouble());
-		audio.header.frame_id =  ros::this_node::getNamespace().substr(1,std::string::npos) + "/microphone_frame";
-		audio.beamAngle = jsonObject["beamAngle"].asDouble();
-		audio.beamAngleConfidence = jsonObject["beamAngleConfidence"].asDouble();
-		for(int i=0;i<256;i++)
-		{
-			audio.audioStream.push_back(jsonObject["audioStream"][i].asFloat());
-		}
-		audio.numBytesPerSample = jsonObject["numBytesPerSample"].asUInt();
-		audio.numSamplesPerFrame = jsonObject["numSamplesPerFrame"].asUInt();
-		audio.frameLifeTime = jsonObject["frameLifeTime"].asDouble();
-		audio.samplingFrequency = jsonObject["samplingFrequency"].asUInt();
-		audioPub.publish(audio);
+        double angle = *((double *)mySocket.mBuffer);
+        std_msgs::Float64 angle_data;
+        angle_data.data = angle;
+		audioPub.publish(angle_data);
 	}
 	return 0;
 }

src/startBody.cpp

@@ -28,34 +28,25 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 #include "k2_client/k2_client.h"
 #include "k2_client/BodyArray.h"
 #include <iconv.h>
+#include <cstdio>
 
 std::string topicName = "bodyArray";
-size_t streamSize = 56008;
-size_t readSkipSize = 56000;
-size_t stringSize = 28000;
+size_t streamSize = 60000;
+size_t readSkipSize = 60000;
+size_t stringSize = 30000;
 
-int main(int argC,char **argV)
+int main(int argc,char **argv)
 {
-    ros::init(argC,argV,"startBody");
+    ros::init(argc,argv,"startBody");
     ros::NodeHandle n;
     std::string serverAddress;
     n.getParam("/serverNameOrIP",serverAddress);
     Socket mySocket(serverAddress.c_str(),"9003",streamSize);
-    iconv_t charConverter = iconv_open("UTF-8","UTF-16");
     ros::Publisher bodyPub = n.advertise<k2_client::BodyArray>(topicName,1);
-    char jsonCharArray[readSkipSize];
-
     while(ros::ok())
     {
         mySocket.readData();        
-        char *jsonCharArrayPtr;
-        char *socketCharArrayPtr;
-        jsonCharArrayPtr = jsonCharArray;
-        socketCharArrayPtr = mySocket.mBuffer;
-        iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize);
-        double utcTime;
-        memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double));
-        std::string jsonString(jsonCharArray);
+        std::string jsonString(mySocket.mBuffer);
         Json::Value jsonObject;
         Json::Reader jsonReader;
         bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
@@ -70,27 +61,34 @@ int main(int argC,char **argV)
             for(int i=0;i<6;i++)
             {
                 k2_client::Body body;
-                body.header.stamp = ros::Time(utcTime);
+                body.header.stamp = ros::Time::now();
                 body.header.frame_id =  ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
-                body.leanTrackingState = jsonObject[i]["LeanTrackingState"].asInt();
-                body.lean.leanX = jsonObject[i]["Lean"]["X"].asDouble();
-                body.lean.leanY = jsonObject[i]["Lean"]["Y"].asDouble();
-                body.isTracked = jsonObject[i]["IsTracked"].asBool();
-                body.trackingId = jsonObject[i]["TrackingId"].asUInt64();
-                body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt();
-                body.engaged = jsonObject[i]["Engaged"].asBool();
-                body.handRightConfidence = jsonObject[i]["HandRightConfidence"].asInt();
-                body.handRightState = jsonObject[i]["HandRightState"].asInt();
-                body.handLeftConfidence = jsonObject[i]["HandLeftConfidence"].asInt();
-                body.handLeftState = jsonObject[i]["HandLeftState"].asInt();
-                body.appearance.wearingGlasses = jsonObject[i]["Appearance"]["WearingGlasses"].asBool();
-                body.activities.eyeLeftClosed = jsonObject[i]["Activities"]["EyeLeftClosed"].asBool();
-                body.activities.eyeRightClosed = jsonObject[i]["Activities"]["EyeRightClosed"].asBool();
-                body.activities.mouthOpen = jsonObject[i]["Activities"]["MouthOpen"].asBool();
-                body.activities.mouthMoved = jsonObject[i]["Activities"]["MouthMoved"].asBool();
-                body.activities.lookingAway = jsonObject[i]["Activities"]["LookingAway"].asBool();
-                body.expressions.neutral = jsonObject[i]["Expressions"]["Neutral"].asBool();
-                body.expressions.neutral = jsonObject[i]["Expressions"]["Happy"].asBool();
+                body.fromX = jsonObject[i]["FromX"].asInt();
+                body.fromY = jsonObject[i]["FromY"].asInt();
+                body.toX = jsonObject[i]["ToX"].asInt();
+                body.toY = jsonObject[i]["ToY"].asInt();
+                body.leanTrackingState = jsonObject[i]["Body"]["LeanTrackingState"].asInt();
+                body.lean.leanX = jsonObject[i]["Body"]["Lean"]["X"].asDouble();
+                body.lean.leanY = jsonObject[i]["Body"]["Lean"]["Y"].asDouble();
+                body.isTracked = jsonObject[i]["Body"]["IsTracked"].asBool();
+                if (!body.isTracked) {
+                    continue;
+                }
+                body.trackingId = jsonObject[i]["Body"]["TrackingId"].asUInt64();
+                body.clippedEdges = jsonObject[i]["Body"]["ClippedEdges"].asInt();
+                body.engaged = jsonObject[i]["Body"]["Engaged"].asBool();
+                body.handRightConfidence = jsonObject[i]["Body"]["HandRightConfidence"].asInt();
+                body.handRightState = jsonObject[i]["Body"]["HandRightState"].asInt();
+                body.handLeftConfidence = jsonObject[i]["Body"]["HandLeftConfidence"].asInt();
+                body.handLeftState = jsonObject[i]["Body"]["HandLeftState"].asInt();
+                body.appearance.wearingGlasses = jsonObject[i]["Body"]["Appearance"]["WearingGlasses"].asBool();
+                body.activities.eyeLeftClosed = jsonObject[i]["Body"]["Activities"]["EyeLeftClosed"].asBool();
+                body.activities.eyeRightClosed = jsonObject[i]["Body"]["Activities"]["EyeRightClosed"].asBool();
+                body.activities.mouthOpen = jsonObject[i]["Body"]["Activities"]["MouthOpen"].asBool();
+                body.activities.mouthMoved = jsonObject[i]["Body"]["Activities"]["MouthMoved"].asBool();
+                body.activities.lookingAway = jsonObject[i]["Body"]["Activities"]["LookingAway"].asBool();
+                body.expressions.neutral = jsonObject[i]["Body"]["Expressions"]["Neutral"].asBool();
+                body.expressions.neutral = jsonObject[i]["Body"]["Expressions"]["Happy"].asBool();
                 for(int j=0;j<25;j++)
                 {
                     k2_client::JointOrientationAndType JOAT;
@@ -125,17 +123,17 @@ int main(int argC,char **argV)
                         case 24: fieldName = "ThumbRight";break;
                     }
 
-                    JOAT.orientation.x = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble();
-                    JOAT.orientation.y = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble();
-                    JOAT.orientation.z = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble();
-                    JOAT.orientation.w = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble();
-                    JOAT.jointType = jsonObject[i]["JointOrientations"][fieldName]["JointType"].asInt();
+                    JOAT.orientation.x = jsonObject[i]["Body"]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble();
+                    JOAT.orientation.y = jsonObject[i]["Body"]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble();
+                    JOAT.orientation.z = jsonObject[i]["Body"]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble();
+                    JOAT.orientation.w = jsonObject[i]["Body"]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble();
+                    JOAT.jointType = jsonObject[i]["Body"]["JointOrientations"][fieldName]["JointType"].asInt();
 
-                    JPAS.trackingState = jsonObject[i]["Joints"][fieldName]["TrackingState"].asBool();
-                    JPAS.position.x = jsonObject[i]["Joints"][fieldName]["Position"]["X"].asDouble();
-                    JPAS.position.y = jsonObject[i]["Joints"][fieldName]["Position"]["Y"].asDouble();
-                    JPAS.position.z = jsonObject[i]["Joints"][fieldName]["Position"]["Z"].asDouble();
-                    JPAS.jointType = jsonObject[i]["Joints"][fieldName]["JointType"].asInt();
+                    JPAS.trackingState = jsonObject[i]["Body"]["Joints"][fieldName]["TrackingState"].asBool();
+                    JPAS.position.x = jsonObject[i]["Body"]["Joints"][fieldName]["Position"]["X"].asDouble();
+                    JPAS.position.y = jsonObject[i]["Body"]["Joints"][fieldName]["Position"]["Y"].asDouble();
+                    JPAS.position.z = jsonObject[i]["Body"]["Joints"][fieldName]["Position"]["Z"].asDouble();
+                    JPAS.jointType = jsonObject[i]["Body"]["Joints"][fieldName]["JointType"].asInt();
 
                     body.jointOrientations.push_back(JOAT);
                     body.jointPositions.push_back(JPAS);
@@ -145,10 +143,11 @@ int main(int argC,char **argV)
         }
         catch (...)
         {
-            ROS_ERROR("An exception occured");
+            ROS_ERROR("startBody - An exception occured");
             continue;
         }
-        bodyPub.publish(bodyArray);
+        if (bodyArray.bodies.size() > 0) 
+            bodyPub.publish(bodyArray);
     }
     return 0;
 }

src/startDepth.cpp

@@ -29,46 +29,42 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 
 // this alternate resolution for an aligned depth image
 //int imageSize = 639392;
-int imageSize = 434176;
-int streamSize = imageSize + sizeof(double);
+int imageSize = 1920 * 1080 * 6;
+int streamSize = imageSize;
 std::string cameraName = "depth";
 std::string imageTopicSubName = "image_depth";
 std::string cameraFrame = "";
 
 int main(int argC,char **argV)
 {
-	ros::init(argC,argV,"startDepth");
-	ros::NodeHandle n(cameraName);
-	image_transport::ImageTransport imT(n);
-	std::string serverAddress;
-	n.getParam("/serverNameOrIP",serverAddress);
+    ros::init(argC,argV,"startDepth");
+    ros::NodeHandle n(cameraName);
+    image_transport::ImageTransport imT(n);
+    std::string serverAddress;
+    n.getParam("/serverNameOrIP",serverAddress);
     n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
             "/depth_frame", cameraFrame);
-	Socket mySocket(serverAddress.c_str(),"9001",streamSize);
-    image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
-            imageTopicSubName, 1);
-	camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
-	camInfoMgr.loadCameraInfo("");
-	cv::Mat frame;
-	cv_bridge::CvImage cvImage;
-	sensor_msgs::Image rosImage;
-	while(ros::ok())
-	{
-		mySocket.readData();
-        // this alternate resolution was for an aligned depth image
-		//frame = cv::Mat(cv::Size(754,424),CV_16UC1,mySocket.mBuffer);
-        frame = cv::Mat(cv::Size(512,424), CV_16UC1,mySocket.mBuffer);
-		cv::flip(frame,frame,1);
-		double utcTime;
-		memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double));
+    Socket mySocket(serverAddress.c_str(), "18000", streamSize);
+    image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName, 1);
+    cv::Mat frame;
+    cv_bridge::CvImage cvImage;
+    sensor_msgs::Image rosImage;
+    while(ros::ok())
+    {
+        mySocket.readData();
+//      for(int i=0; i < 6; i++) {
+//          printf("%02x", (unsigned char)(mySocket.mBuffer[(1920 * 1079 + 1000) * 6 + i]));
+//      }
+//      printf("\n");
+        frame = cv::Mat(cv::Size(1920,1080), CV_16SC3,mySocket.mBuffer);
+		cv::flip(frame,frame,0);
+        cv::Vec3s test_vec = frame.at<cv::Vec3s>(500, 500);
         cvImage.header.frame_id = cameraFrame.c_str();
-		cvImage.encoding = "16UC1";
+		cvImage.encoding = "16SC3";
 		cvImage.image = frame;
 		cvImage.toImageMsg(rosImage);
-		sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
-		camInfo.header.frame_id = cvImage.header.frame_id;
-        cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
+        imagePublisher.publish(rosImage);
 		ros::spinOnce();
-	}
-	return 0;
+    }
+    return 0;
 }