Feature/k2 client

launch/k2_client.launch

@@ -1,5 +1,6 @@
 <launch>
-    <param name="serverNameOrIP" value="172.19.179.123" />
+    <param name="serverNameOrIP" value="172.19.179.53" />
+
     <group ns="/head/kinect2">
         <param name="rgb_frame" value="/head/kinect2/rgb"/>
         <param name="depth_frame" value="/head/kinect2/depth"/>
@@ -12,3 +13,5 @@
         <node name="startAudio" pkg="k2_client" type="startAudio"/>
     </group>
 </launch>
+
+

package.xml

@@ -22,6 +22,7 @@
   <depend>cv_bridge</depend>
   <depend>image_transport</depend>
   <depend>camera_info_manager</depend> 
+  <depend>jsoncpp</depend> 
 
 </package>
 

src/startBody.cpp

@@ -30,9 +30,9 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 #include <iconv.h>
 
 std::string topicName = "bodyArray";
-size_t streamSize = 56008;
-size_t readSkipSize = 56000;
-size_t stringSize = 28000;
+size_t streamSize = 80008;
+size_t readSkipSize = 80000;
+size_t stringSize = 40000;
 
 int main(int argC,char **argV)
 {
@@ -44,29 +44,36 @@ int main(int argC,char **argV)
     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();        
+        mySocket.readData();  
         char *jsonCharArrayPtr;
         char *socketCharArrayPtr;
+        size_t readSkipSizeC = readSkipSize;
+        size_t stringSizeloC= stringSize;
         jsonCharArrayPtr = jsonCharArray;
-        socketCharArrayPtr = mySocket.mBuffer;
-        iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize);
+        socketCharArrayPtr = mySocket.mBuffer;        
+        iconv(charConverter,&socketCharArrayPtr,&readSkipSizeC,&jsonCharArrayPtr,&stringSizeloC);
         double utcTime;
-        memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double));
+        memcpy(&utcTime,&mySocket.mBuffer[readSkipSizeC],sizeof(double));
         std::string jsonString(jsonCharArray);
         Json::Value jsonObject;
         Json::Reader jsonReader;
         bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
+
         if(!parsingSuccessful)
         {
             ROS_ERROR("Failure to parse");
-            continue;
         }
+
+        if (jsonObject.size() != 6)
+                    continue;
+
         k2_client::BodyArray bodyArray;
         try
         {
+            //ROS_ERROR("Parsing successulff");
             for(int i=0;i<6;i++)
             {
                 k2_client::Body body;
@@ -124,13 +131,13 @@ int main(int argC,char **argV)
                         case 23: fieldName = "HandTipRight";break;
                         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();
-
+                    
                     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();
@@ -140,6 +147,7 @@ int main(int argC,char **argV)
                     body.jointOrientations.push_back(JOAT);
                     body.jointPositions.push_back(JPAS);
                 }
+                //std::cout << ros::Time::now() <<std::endl;
                 bodyArray.bodies.push_back(body);
             }
         }
@@ -149,6 +157,7 @@ int main(int argC,char **argV)
             continue;
         }
         bodyPub.publish(bodyArray);
+
     }
     return 0;
-}
+}

src/startDepth.cpp

@@ -42,11 +42,11 @@ int main(int argC,char **argV)
 	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);
+	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);
+	image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
+				imageTopicSubName, 1);
 	camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
 	camInfoMgr.loadCameraInfo("");
 	cv::Mat frame;
@@ -55,19 +55,19 @@ int main(int argC,char **argV)
 	while(ros::ok())
 	{
 		mySocket.readData();
-        // this alternate resolution was for an aligned depth image
+		// 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);
+		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));
-        cvImage.header.frame_id = cameraFrame.c_str();
+		cvImage.header.frame_id = cameraFrame.c_str();
 		cvImage.encoding = "16UC1";
 		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));
+		cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
 		ros::spinOnce();
 	}
 	return 0;

src/startIR.cpp

@@ -40,11 +40,11 @@ int main(int argC,char **argV)
 	image_transport::ImageTransport imT(n);
 	std::string serverAddress;
 	n.getParam("/serverNameOrIP",serverAddress);
-    n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
-            "/ir_frame", cameraFrame);
+	n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
+				"/ir_frame", cameraFrame);
 	Socket mySocket(serverAddress.c_str(),"9002",streamSize);
-    image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
-            imageTopicSubName, 1);
+	image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
+				imageTopicSubName, 1);
 	camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName);
 	camInfoMgr.loadCameraInfo("");
 	cv::Mat frame;
@@ -63,7 +63,7 @@ int main(int argC,char **argV)
 		cvImage.toImageMsg(rosImage);
 		sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
 		camInfo.header.frame_id = cvImage.header.frame_id;
-        cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
+		cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
 		ros::spinOnce();
 	}
 	return 0;

src/startRGB.cpp

@@ -27,7 +27,7 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
 ***************************************************************************************/
 #include "k2_client/k2_client.h"
 
-int imageSize = 6220800;
+int imageSize = 8294400;
 int streamSize = imageSize + sizeof(double);
 std::string cameraName = "rgb";
 std::string imageTopicSubName = "image_color";
@@ -40,35 +40,35 @@ int main(int argC,char **argV)
 	image_transport::ImageTransport imT(n);
 	std::string serverAddress;
 	n.getParam("/serverNameOrIP",serverAddress);
-    n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
-            "/rgb_frame", cameraFrame);
+	n.getParam(ros::this_node::getNamespace().substr(1,std::string::npos) +
+	"/rgb_frame", cameraFrame);
 	Socket mySocket(serverAddress.c_str(),"9000",streamSize);
-    image_transport::CameraPublisher cameraPublisher = imT.advertiseCamera(
-            imageTopicSubName, 1);
+	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())
 	{
-        printf("Got a frame.\n");
-
+		printf("Got a frame.\n");
+		
 		mySocket.readData();
-        printf("Creating mat.\n");
-        frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer);
+		printf("Creating mat.\n");
+		frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer);
 		cv::flip(frame,frame,1);
-        printf("Getting time.\n");
+		printf("Getting time.\n");
 		double utcTime;
 		memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double));
-        cvImage.header.frame_id = cameraFrame.c_str();
+		cvImage.header.frame_id = cameraFrame.c_str();
 		cvImage.encoding = "bgr8";
 		cvImage.image = frame;
 		cvImage.toImageMsg(rosImage);
 		sensor_msgs::CameraInfo camInfo = camInfoMgr.getCameraInfo();
 		camInfo.header.frame_id = cvImage.header.frame_id;
-        printf("Updating.\n");
-        cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
+		printf("Updating.\n");
+		cameraPublisher.publish(rosImage, camInfo, ros::Time(utcTime));
 		ros::spinOnce();
 	}
 	return 0;