From 474c52ab49d05407eabba38199118c3a493855ac Mon Sep 17 00:00:00 2001 From: gjc13 Date: Thu, 17 Mar 2016 05:10:41 +0000 Subject: [PATCH 01/14] [Fix] RGB data corrupt --- launch/k2_client.launch | 2 +- src/startRGB.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 4b23c9d..f691b31 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -1,5 +1,5 @@ - + diff --git a/src/startRGB.cpp b/src/startRGB.cpp index a66bf3e..f7e6eae 100644 --- a/src/startRGB.cpp +++ b/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 = 1920 * 1080 * 4; int streamSize = imageSize + sizeof(double); std::string cameraName = "rgb"; std::string imageTopicSubName = "image_color"; @@ -56,12 +56,14 @@ int main(int argC,char **argV) mySocket.readData(); printf("Creating mat.\n"); - frame = cv::Mat(cv::Size(1920,1080), CV_8UC3, mySocket.mBuffer); + frame = cv::Mat(cv::Size(1920, 1080), CV_8UC4, mySocket.mBuffer); + cv::cvtColor(frame, frame, CV_BGRA2BGR); cv::flip(frame,frame,1); printf("Getting time.\n"); double utcTime; memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); cvImage.header.frame_id = cameraFrame.c_str(); + printf("%s\n", cameraFrame.c_str()); cvImage.encoding = "bgr8"; cvImage.image = frame; cvImage.toImageMsg(rosImage); From 0fb79b5ac76868e747e6316aa3f1c533e9323ecb Mon Sep 17 00:00:00 2001 From: gjc13 Date: Wed, 6 Apr 2016 13:42:42 +0100 Subject: [PATCH 02/14] [Change] Use new depth frame data format --- launch/k2_client.launch | 7 +++---- src/startDepth.cpp | 28 ++++++++++++---------------- src/startRGB.cpp | 19 +++---------------- 3 files changed, 18 insertions(+), 36 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index f691b31..a807fec 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -6,9 +6,8 @@ - - - - + + diff --git a/src/startDepth.cpp b/src/startDepth.cpp index 2248688..9343e1d 100644 --- a/src/startDepth.cpp +++ b/src/startDepth.cpp @@ -29,8 +29,8 @@ 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 = ""; @@ -44,30 +44,26 @@ int main(int argC,char **argV) 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(""); + 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(); - // 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); +// 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,1); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); + cv::Vec3s test_vec = frame.at(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; diff --git a/src/startRGB.cpp b/src/startRGB.cpp index f7e6eae..799808c 100644 --- a/src/startRGB.cpp +++ b/src/startRGB.cpp @@ -28,7 +28,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 = 1920 * 1080 * 4; -int streamSize = imageSize + sizeof(double); +int streamSize = imageSize; std::string cameraName = "rgb"; std::string imageTopicSubName = "image_color"; std::string cameraFrame = ""; @@ -43,34 +43,21 @@ int main(int argC,char **argV) 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); - camera_info_manager::CameraInfoManager camInfoMgr(n,cameraName); - camInfoMgr.loadCameraInfo(""); + image_transport::Publisher imagePublisher = imT.advertise(imageTopicSubName, 1); cv::Mat frame; cv_bridge::CvImage cvImage; sensor_msgs::Image rosImage; while(ros::ok()) { - printf("Got a frame.\n"); - mySocket.readData(); - printf("Creating mat.\n"); frame = cv::Mat(cv::Size(1920, 1080), CV_8UC4, mySocket.mBuffer); cv::cvtColor(frame, frame, CV_BGRA2BGR); cv::flip(frame,frame,1); - printf("Getting time.\n"); - double utcTime; - memcpy(&utcTime,&mySocket.mBuffer[imageSize],sizeof(double)); cvImage.header.frame_id = cameraFrame.c_str(); - printf("%s\n", 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)); + imagePublisher.publish(rosImage); ros::spinOnce(); } return 0; From fa5d3a16d53df6f6134b168353322ca0dffa318d Mon Sep 17 00:00:00 2001 From: gjc13 Date: Wed, 27 Apr 2016 06:41:06 +0000 Subject: [PATCH 03/14] [Add] read body frame --- launch/k2_client.launch | 5 ++--- src/startBody.cpp | 27 +++++++++++---------------- 2 files changed, 13 insertions(+), 19 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index a807fec..21694dd 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -1,5 +1,5 @@ - + @@ -7,7 +7,6 @@ - + diff --git a/src/startBody.cpp b/src/startBody.cpp index 1be4a64..3789ca2 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -28,11 +28,12 @@ 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 +#include 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) { @@ -41,21 +42,11 @@ int main(int argC,char **argV) 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(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,12 +61,15 @@ 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(); + if (!body.isTracked) { + continue; + } body.trackingId = jsonObject[i]["TrackingId"].asUInt64(); body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt(); body.engaged = jsonObject[i]["Engaged"].asBool(); @@ -148,7 +142,8 @@ int main(int argC,char **argV) ROS_ERROR("An exception occured"); continue; } - bodyPub.publish(bodyArray); + if (bodyArray.bodies.size() > 0) + bodyPub.publish(bodyArray); } return 0; } From 726195c8fdb29bd21da4b1eb7e2546c45c8d2e82 Mon Sep 17 00:00:00 2001 From: yaoht Date: Fri, 29 Apr 2016 13:43:28 +0000 Subject: [PATCH 04/14] [ADD] launch machine conf --- launch/k2_client.launch | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 21694dd..3937836 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -1,12 +1,13 @@ + - - - + + + From b3a251728a7b4eb3f18828669cca22d3a68a7ada Mon Sep 17 00:00:00 2001 From: Guo JiaCheng Date: Tue, 3 May 2016 15:09:59 +0800 Subject: [PATCH 05/14] Change to flip x axis --- src/startDepth.cpp | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/startDepth.cpp b/src/startDepth.cpp index 9343e1d..5990108 100644 --- a/src/startDepth.cpp +++ b/src/startDepth.cpp @@ -37,27 +37,27 @@ 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(), "18000", streamSize); + 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"); + 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,1); + cv::flip(frame,frame,0); cv::Vec3s test_vec = frame.at(500, 500); cvImage.header.frame_id = cameraFrame.c_str(); cvImage.encoding = "16SC3"; @@ -65,6 +65,6 @@ int main(int argC,char **argV) cvImage.toImageMsg(rosImage); imagePublisher.publish(rosImage); ros::spinOnce(); - } - return 0; + } + return 0; } From c20f1ebb03aa2a34879921801ea8f9ebbc1ad395 Mon Sep 17 00:00:00 2001 From: Guo JiaCheng Date: Tue, 3 May 2016 15:11:39 +0800 Subject: [PATCH 06/14] [Change] Change to flip x axis --- src/startRGB.cpp | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/startRGB.cpp b/src/startRGB.cpp index 799808c..a9c3e39 100644 --- a/src/startRGB.cpp +++ b/src/startRGB.cpp @@ -35,30 +35,30 @@ std::string cameraFrame = ""; int main(int argC,char **argV) { - ros::init(argC,argV,"startRGB"); - ros::NodeHandle n(cameraName); - image_transport::ImageTransport imT(n); - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); + ros::init(argC,argV,"startRGB"); + 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) + "/rgb_frame", cameraFrame); - Socket mySocket(serverAddress.c_str(),"9000",streamSize); + Socket mySocket(serverAddress.c_str(),"9000",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(); + cv::Mat frame; + cv_bridge::CvImage cvImage; + sensor_msgs::Image rosImage; + while(ros::ok()) + { + mySocket.readData(); frame = cv::Mat(cv::Size(1920, 1080), CV_8UC4, mySocket.mBuffer); cv::cvtColor(frame, frame, CV_BGRA2BGR); - cv::flip(frame,frame,1); + cv::flip(frame,frame,0); cvImage.header.frame_id = cameraFrame.c_str(); cvImage.encoding = "bgr8"; cvImage.image = frame; cvImage.toImageMsg(rosImage); imagePublisher.publish(rosImage); ros::spinOnce(); - } - return 0; + } + return 0; } From cf94c560638285524d2273f463ea012b796b506d Mon Sep 17 00:00:00 2001 From: sugar10w Date: Sat, 7 May 2016 21:15:49 +0800 Subject: [PATCH 07/14] [Add][Debugging] startFace --- CMakeLists.txt | 11 ++++- launch/k2_client.launch | 1 + msg/BoundingBox2D.msg | 4 ++ msg/Face.msg | 16 +++++++ msg/FaceArray.msg | 1 + msg/FacePoints.msg | 10 ++++ msg/Quaternion.msg | 4 ++ src/startFace.cpp | 101 ++++++++++++++++++++++++++++++++++++++++ 8 files changed, 147 insertions(+), 1 deletion(-) create mode 100644 msg/BoundingBox2D.msg create mode 100644 msg/Face.msg create mode 100644 msg/FaceArray.msg create mode 100644 msg/FacePoints.msg create mode 100644 msg/Quaternion.msg create mode 100644 src/startFace.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b3a91b..8a67f7f 100644 --- a/CMakeLists.txt +++ b/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}) + diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 3937836..b645f22 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -9,5 +9,6 @@ + diff --git a/msg/BoundingBox2D.msg b/msg/BoundingBox2D.msg new file mode 100644 index 0000000..006ac1d --- /dev/null +++ b/msg/BoundingBox2D.msg @@ -0,0 +1,4 @@ +int32 left +int32 top +int32 right +int32 bottom \ No newline at end of file diff --git a/msg/Face.msg b/msg/Face.msg new file mode 100644 index 0000000..d367cc6 --- /dev/null +++ b/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 diff --git a/msg/FaceArray.msg b/msg/FaceArray.msg new file mode 100644 index 0000000..459c992 --- /dev/null +++ b/msg/FaceArray.msg @@ -0,0 +1 @@ +Face[] faces \ No newline at end of file diff --git a/msg/FacePoints.msg b/msg/FacePoints.msg new file mode 100644 index 0000000..81a29a7 --- /dev/null +++ b/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 diff --git a/msg/Quaternion.msg b/msg/Quaternion.msg new file mode 100644 index 0000000..f908ddc --- /dev/null +++ b/msg/Quaternion.msg @@ -0,0 +1,4 @@ +float32 X +float32 Y +float32 Z +float32 W \ No newline at end of file diff --git a/src/startFace.cpp b/src/startFace.cpp new file mode 100644 index 0000000..e0147a4 --- /dev/null +++ b/src/startFace.cpp @@ -0,0 +1,101 @@ +#include "k2_client/k2_client.h" +#include "k2_client/FaceArray.h" +#include +#include + +std::string topicName = "faceArray"; +size_t streamSize = 60000; +size_t readSkipSize = 60000; +size_t stringSize = 30000; + +int main(int argC,char **argV) +{ + ros::init(argC,argV,"startFace"); + ros::NodeHandle n; + std::string serverAddress; + n.getParam("/serverNameOrIP",serverAddress); + Socket mySocket(serverAddress.c_str(),"9006",streamSize); + ros::Publisher facePub = n.advertise(topicName,1); + while(ros::ok()) + { + mySocket.readData(); + std::string jsonString(mySocket.mBuffer); + Json::Value jsonObject; + Json::Reader jsonReader; + bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false); + if(!parsingSuccessful) + { + ROS_ERROR("Failure to parse"); + continue; + } + k2_client::FaceArray faceArray; + try + { + for(int i=0;i<6;i++) + { + k2_client::Face face; + face.header.stamp = ros::Time::now(); + face.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame"; + + face.appearance.wearingGlasses = jsonObject[i]["FaceProperties"]["WearingGlasses"].asBool(); + face.activities.eyeLeftClosed = jsonObject[i]["FaceProperties"]["EyeLeftClosed"].asBool(); + face.activities.eyeRightClosed = jsonObject[i]["FaceProperties"]["EyeRightClosed"].asBool(); + face.activities.mouthOpen = jsonObject[i]["FaceProperties"]["MouthOpen"].asBool(); + face.activities.mouthMoved = jsonObject[i]["FaceProperties"]["MouthMoved"].asBool(); + face.activities.lookingAway = jsonObject[i]["FaceProperties"]["LookingAway"].asBool(); + face.expressions.neutral = jsonObject[i]["FaceProperties"]["Neutral"].asBool(); + face.expressions.neutral = jsonObject[i]["FaceProperties"]["Happy"].asBool(); + + face.facePointsInInfraredSpace.eyeLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["X"]; + face.facePointsInInfraredSpace.eyeLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["Y"]; + face.facePointsInInfraredSpace.eyeRightX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"]; + face.facePointsInInfraredSpace.eyeRightY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"]; + face.facePointsInInfraredSpace.noseX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"]; + face.facePointsInInfraredSpace.noseY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"]; + face.facePointsInInfraredSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["X"]; + face.facePointsInInfraredSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["Y"]; + face.facePointsInInfraredSpace.mouthCornerRightX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["X"]; + face.facePointsInInfraredSpace.mouthCornerRightY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["Y"]; + + face.facePointsInColorSpace.eyeLeftX = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["X"]; + face.facePointsInColorSpace.eyeLeftY = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["Y"]; + face.facePointsInColorSpace.eyeRightX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"]; + face.facePointsInColorSpace.eyeRightY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"]; + face.facePointsInColorSpace.noseX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"]; + face.facePointsInColorSpace.noseY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"]; + face.facePointsInColorSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["X"]; + face.facePointsInColorSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["Y"]; + face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"]; + face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"]; + + face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"]; + face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"]; + face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"]; + face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"]; + + face.FaceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"]; + face.FaceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"]; + face.FaceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"]; + face.FaceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"]; + + face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"]; + face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"]; + face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"]; + face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"]; + + face.trackingId = jsonObject[i]["TrackingId"]; + face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"]; + + faceArray.faces.push_back(face); + } + } + catch (...) + { + ROS_ERROR("An exception occured"); + continue; + } + if (faceArray.faces.size() > 0) + facePub.publish(faceArray); + } + return 0; +} From 47d9f91680710bd42974069bcc3a9fd5f4cf579a Mon Sep 17 00:00:00 2001 From: sugar10w Date: Sat, 7 May 2016 22:06:00 +0800 Subject: [PATCH 08/14] [debugging] startFace --- src/startFace.cpp | 68 +++++++++++++++++++++++------------------------ 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/src/startFace.cpp b/src/startFace.cpp index e0147a4..244c84c 100644 --- a/src/startFace.cpp +++ b/src/startFace.cpp @@ -46,45 +46,45 @@ int main(int argC,char **argV) face.expressions.neutral = jsonObject[i]["FaceProperties"]["Neutral"].asBool(); face.expressions.neutral = jsonObject[i]["FaceProperties"]["Happy"].asBool(); - face.facePointsInInfraredSpace.eyeLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["X"]; - face.facePointsInInfraredSpace.eyeLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["Y"]; - face.facePointsInInfraredSpace.eyeRightX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"]; - face.facePointsInInfraredSpace.eyeRightY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"]; - face.facePointsInInfraredSpace.noseX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"]; - face.facePointsInInfraredSpace.noseY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"]; - face.facePointsInInfraredSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["X"]; - face.facePointsInInfraredSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["Y"]; - face.facePointsInInfraredSpace.mouthCornerRightX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["X"]; - face.facePointsInInfraredSpace.mouthCornerRightY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["Y"]; + face.facePointsInInfraredSpace.eyeLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["X"].asDouble(); + face.facePointsInInfraredSpace.eyeLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["Y"].asDouble(); + face.facePointsInInfraredSpace.eyeRightX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInInfraredSpace.eyeRightY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInInfraredSpace.noseX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInInfraredSpace.noseY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["X"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["Y"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerRightX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["X"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerRightY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["Y"].asDouble(); - face.facePointsInColorSpace.eyeLeftX = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["X"]; - face.facePointsInColorSpace.eyeLeftY = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["Y"]; - face.facePointsInColorSpace.eyeRightX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"]; - face.facePointsInColorSpace.eyeRightY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"]; - face.facePointsInColorSpace.noseX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"]; - face.facePointsInColorSpace.noseY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"]; - face.facePointsInColorSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["X"]; - face.facePointsInColorSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["Y"]; - face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"]; - face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"]; + face.facePointsInColorSpace.eyeLeftX = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["X"].asDouble(); + face.facePointsInColorSpace.eyeLeftY = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["Y"].asDouble(); + face.facePointsInColorSpace.eyeRightX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInColorSpace.eyeRightY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInColorSpace.noseX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInColorSpace.noseY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInColorSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["X"].asDouble(); + face.facePointsInColorSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["Y"].asDouble(); + face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"].asDouble(); + face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"].asDouble(); - face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"]; - face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"]; - face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"]; - face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"]; + face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"].asDouble(); + face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"].asDouble(); + face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"].asDouble(); + face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"].asDouble(); - face.FaceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"]; - face.FaceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"]; - face.FaceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"]; - face.FaceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"]; + face.FaceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"].asDouble(); + face.FaceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"].asDouble(); + face.FaceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"].asDouble(); + face.FaceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"].asDouble(); - face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"]; - face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"]; - face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"]; - face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"]; + face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"].asDouble(); + face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"].asDouble(); + face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"].asDouble(); + face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"].asDouble(); - face.trackingId = jsonObject[i]["TrackingId"]; - face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"]; + face.trackingId = jsonObject[i]["TrackingId"].asInt(); + face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"].asInt(); faceArray.faces.push_back(face); } From 9de05014135d4ec4fd98b35fd6cbbbf6e66cd890 Mon Sep 17 00:00:00 2001 From: furoc Date: Sat, 7 May 2016 14:41:53 +0000 Subject: [PATCH 09/14] [Add] startFace finished --- msg/Face.msg | 2 +- src/startFace.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/msg/Face.msg b/msg/Face.msg index d367cc6..92c4d5c 100644 --- a/msg/Face.msg +++ b/msg/Face.msg @@ -8,7 +8,7 @@ FacePoints facePointsInInfraredSpace FacePoints facePointsInColorSpace BoundingBox2D faceBoundingBoxInInfraredSpace -BoundingBox2D FaceBoundingBoxInColorSpace +BoundingBox2D faceBoundingBoxInColorSpace Quaternion faceRotationQuaternion diff --git a/src/startFace.cpp b/src/startFace.cpp index 244c84c..d9da846 100644 --- a/src/startFace.cpp +++ b/src/startFace.cpp @@ -68,22 +68,22 @@ int main(int argC,char **argV) face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"].asDouble(); face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"].asDouble(); - face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"].asDouble(); - face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"].asDouble(); - face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"].asDouble(); - face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"].asDouble(); + face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"].asInt(); + face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"].asInt(); + face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"].asInt(); + face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"].asInt(); - face.FaceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"].asDouble(); - face.FaceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"].asDouble(); - face.FaceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"].asDouble(); - face.FaceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"].asDouble(); + face.faceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"].asInt(); + face.faceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"].asInt(); + face.faceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"].asInt(); + face.faceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"].asInt(); face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"].asDouble(); face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"].asDouble(); face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"].asDouble(); face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"].asDouble(); - face.trackingId = jsonObject[i]["TrackingId"].asInt(); + //face.trackingId = jsonObject[i]["TrackingId"].asInt(); face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"].asInt(); faceArray.faces.push_back(face); From 39ad1ac3c9811cc45708062bc88777d566880981 Mon Sep 17 00:00:00 2001 From: yht1995 Date: Mon, 16 May 2016 13:19:14 +0000 Subject: [PATCH 10/14] [Add] Kinect body bounding box --- launch/k2_client.launch | 5 ++- msg/Body.msg | 6 ++- msg/Face.msg | 2 +- src/startBody.cpp | 44 +++++++++++--------- src/startFace.cpp | 91 +---------------------------------------- 5 files changed, 35 insertions(+), 113 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index b645f22..8a52593 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -9,6 +9,7 @@ - - + <-node machine="hyperv-linux" name="startFace" pkg="k2_client" type="startFace" output="screen"/> + + diff --git a/msg/Body.msg b/msg/Body.msg index ad84f09..835f6b7 100644 --- a/msg/Body.msg +++ b/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 \ No newline at end of file +JointPositionAndState[] jointPositions diff --git a/msg/Face.msg b/msg/Face.msg index d367cc6..92c4d5c 100644 --- a/msg/Face.msg +++ b/msg/Face.msg @@ -8,7 +8,7 @@ FacePoints facePointsInInfraredSpace FacePoints facePointsInColorSpace BoundingBox2D faceBoundingBoxInInfraredSpace -BoundingBox2D FaceBoundingBoxInColorSpace +BoundingBox2D faceBoundingBoxInColorSpace Quaternion faceRotationQuaternion diff --git a/src/startBody.cpp b/src/startBody.cpp index 3789ca2..8a7daae 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -63,28 +63,32 @@ int main(int argC,char **argV) k2_client::Body body; 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.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]["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.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; @@ -139,7 +143,7 @@ int main(int argC,char **argV) } catch (...) { - ROS_ERROR("An exception occured"); + ROS_ERROR("startBody - An exception occured"); continue; } if (bodyArray.bodies.size() > 0) diff --git a/src/startFace.cpp b/src/startFace.cpp index 244c84c..e876f0e 100644 --- a/src/startFace.cpp +++ b/src/startFace.cpp @@ -3,99 +3,12 @@ #include #include -std::string topicName = "faceArray"; -size_t streamSize = 60000; -size_t readSkipSize = 60000; -size_t stringSize = 30000; - int main(int argC,char **argV) { - ros::init(argC,argV,"startFace"); - ros::NodeHandle n; - std::string serverAddress; - n.getParam("/serverNameOrIP",serverAddress); - Socket mySocket(serverAddress.c_str(),"9006",streamSize); - ros::Publisher facePub = n.advertise(topicName,1); while(ros::ok()) { - mySocket.readData(); - std::string jsonString(mySocket.mBuffer); - Json::Value jsonObject; - Json::Reader jsonReader; - bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false); - if(!parsingSuccessful) - { - ROS_ERROR("Failure to parse"); - continue; - } - k2_client::FaceArray faceArray; - try - { - for(int i=0;i<6;i++) - { - k2_client::Face face; - face.header.stamp = ros::Time::now(); - face.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame"; - - face.appearance.wearingGlasses = jsonObject[i]["FaceProperties"]["WearingGlasses"].asBool(); - face.activities.eyeLeftClosed = jsonObject[i]["FaceProperties"]["EyeLeftClosed"].asBool(); - face.activities.eyeRightClosed = jsonObject[i]["FaceProperties"]["EyeRightClosed"].asBool(); - face.activities.mouthOpen = jsonObject[i]["FaceProperties"]["MouthOpen"].asBool(); - face.activities.mouthMoved = jsonObject[i]["FaceProperties"]["MouthMoved"].asBool(); - face.activities.lookingAway = jsonObject[i]["FaceProperties"]["LookingAway"].asBool(); - face.expressions.neutral = jsonObject[i]["FaceProperties"]["Neutral"].asBool(); - face.expressions.neutral = jsonObject[i]["FaceProperties"]["Happy"].asBool(); - - face.facePointsInInfraredSpace.eyeLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["X"].asDouble(); - face.facePointsInInfraredSpace.eyeLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["Y"].asDouble(); - face.facePointsInInfraredSpace.eyeRightX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInInfraredSpace.eyeRightY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInInfraredSpace.noseX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInInfraredSpace.noseY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["X"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["Y"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerRightX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["X"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerRightY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["Y"].asDouble(); - - face.facePointsInColorSpace.eyeLeftX = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["X"].asDouble(); - face.facePointsInColorSpace.eyeLeftY = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["Y"].asDouble(); - face.facePointsInColorSpace.eyeRightX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInColorSpace.eyeRightY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInColorSpace.noseX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInColorSpace.noseY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInColorSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["X"].asDouble(); - face.facePointsInColorSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["Y"].asDouble(); - face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"].asDouble(); - face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"].asDouble(); - - face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"].asDouble(); - face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"].asDouble(); - face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"].asDouble(); - face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"].asDouble(); - - face.FaceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"].asDouble(); - face.FaceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"].asDouble(); - face.FaceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"].asDouble(); - face.FaceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"].asDouble(); - - face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"].asDouble(); - face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"].asDouble(); - face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"].asDouble(); - face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"].asDouble(); - - face.trackingId = jsonObject[i]["TrackingId"].asInt(); - face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"].asInt(); - - faceArray.faces.push_back(face); - } - } - catch (...) - { - ROS_ERROR("An exception occured"); - continue; - } - if (faceArray.faces.size() > 0) - facePub.publish(faceArray); + ROS_ERROR("STARTFACE is running."); + } return 0; } From cd00180f549d00135be4345bebc18d7d77073f12 Mon Sep 17 00:00:00 2001 From: gjc13 Date: Mon, 16 May 2016 14:29:02 +0000 Subject: [PATCH 11/14] [Debug] Fix body bounding box bugs --- launch/k2_client.launch | 2 +- src/startBody.cpp | 8 +- src/startFace.cpp | 191 +++++++++++++++++++++------------------- 3 files changed, 106 insertions(+), 95 deletions(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 8a52593..2021bff 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -9,7 +9,7 @@ - <-node machine="hyperv-linux" name="startFace" pkg="k2_client" type="startFace" output="screen"/> + diff --git a/src/startBody.cpp b/src/startBody.cpp index 8a7daae..c2d964a 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -35,9 +35,9 @@ 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); @@ -65,8 +65,8 @@ int main(int argC,char **argV) body.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame"; 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.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(); diff --git a/src/startFace.cpp b/src/startFace.cpp index e708b97..d9da846 100644 --- a/src/startFace.cpp +++ b/src/startFace.cpp @@ -1,90 +1,101 @@ -#include "k2_client/k2_client.h" -#include "k2_client/FaceArray.h" -#include -#include - -int main(int argC,char **argV) -{ - while(ros::ok()) - { - mySocket.readData(); - std::string jsonString(mySocket.mBuffer); - Json::Value jsonObject; - Json::Reader jsonReader; - bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false); - if(!parsingSuccessful) - { - ROS_ERROR("Failure to parse"); - continue; - } - k2_client::FaceArray faceArray; - try - { - for(int i=0;i<6;i++) - { - k2_client::Face face; - face.header.stamp = ros::Time::now(); - face.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame"; - - face.appearance.wearingGlasses = jsonObject[i]["FaceProperties"]["WearingGlasses"].asBool(); - face.activities.eyeLeftClosed = jsonObject[i]["FaceProperties"]["EyeLeftClosed"].asBool(); - face.activities.eyeRightClosed = jsonObject[i]["FaceProperties"]["EyeRightClosed"].asBool(); - face.activities.mouthOpen = jsonObject[i]["FaceProperties"]["MouthOpen"].asBool(); - face.activities.mouthMoved = jsonObject[i]["FaceProperties"]["MouthMoved"].asBool(); - face.activities.lookingAway = jsonObject[i]["FaceProperties"]["LookingAway"].asBool(); - face.expressions.neutral = jsonObject[i]["FaceProperties"]["Neutral"].asBool(); - face.expressions.neutral = jsonObject[i]["FaceProperties"]["Happy"].asBool(); - - face.facePointsInInfraredSpace.eyeLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["X"].asDouble(); - face.facePointsInInfraredSpace.eyeLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["Y"].asDouble(); - face.facePointsInInfraredSpace.eyeRightX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInInfraredSpace.eyeRightY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInInfraredSpace.noseX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInInfraredSpace.noseY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["X"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["Y"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerRightX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["X"].asDouble(); - face.facePointsInInfraredSpace.mouthCornerRightY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["Y"].asDouble(); - - face.facePointsInColorSpace.eyeLeftX = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["X"].asDouble(); - face.facePointsInColorSpace.eyeLeftY = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["Y"].asDouble(); - face.facePointsInColorSpace.eyeRightX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInColorSpace.eyeRightY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInColorSpace.noseX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); - face.facePointsInColorSpace.noseY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); - face.facePointsInColorSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["X"].asDouble(); - face.facePointsInColorSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["Y"].asDouble(); - face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"].asDouble(); - face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"].asDouble(); - - face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"].asInt(); - face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"].asInt(); - face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"].asInt(); - face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"].asInt(); - - face.faceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"].asInt(); - face.faceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"].asInt(); - face.faceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"].asInt(); - face.faceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"].asInt(); - - face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"].asDouble(); - face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"].asDouble(); - face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"].asDouble(); - face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"].asDouble(); - - //face.trackingId = jsonObject[i]["TrackingId"].asInt(); - face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"].asInt(); - - faceArray.faces.push_back(face); - } - } - catch (...) - { - ROS_ERROR("An exception occured"); - continue; - } - if (faceArray.faces.size() > 0) - facePub.publish(faceArray); - } - return 0; -} +#include "k2_client/k2_client.h" +#include "k2_client/FaceArray.h" +#include +#include + +std::string topicName = "faceArray"; +size_t streamSize = 60000; +size_t readSkipSize = 60000; +size_t stringSize = 30000; + +int main(int argC,char **argV) +{ + ros::init(argC,argV,"startFace"); + ros::NodeHandle n; + std::string serverAddress; + n.getParam("/serverNameOrIP",serverAddress); + Socket mySocket(serverAddress.c_str(),"9006",streamSize); + ros::Publisher facePub = n.advertise(topicName,1); + while(ros::ok()) + { + mySocket.readData(); + std::string jsonString(mySocket.mBuffer); + Json::Value jsonObject; + Json::Reader jsonReader; + bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false); + if(!parsingSuccessful) + { + ROS_ERROR("Failure to parse"); + continue; + } + k2_client::FaceArray faceArray; + try + { + for(int i=0;i<6;i++) + { + k2_client::Face face; + face.header.stamp = ros::Time::now(); + face.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame"; + + face.appearance.wearingGlasses = jsonObject[i]["FaceProperties"]["WearingGlasses"].asBool(); + face.activities.eyeLeftClosed = jsonObject[i]["FaceProperties"]["EyeLeftClosed"].asBool(); + face.activities.eyeRightClosed = jsonObject[i]["FaceProperties"]["EyeRightClosed"].asBool(); + face.activities.mouthOpen = jsonObject[i]["FaceProperties"]["MouthOpen"].asBool(); + face.activities.mouthMoved = jsonObject[i]["FaceProperties"]["MouthMoved"].asBool(); + face.activities.lookingAway = jsonObject[i]["FaceProperties"]["LookingAway"].asBool(); + face.expressions.neutral = jsonObject[i]["FaceProperties"]["Neutral"].asBool(); + face.expressions.neutral = jsonObject[i]["FaceProperties"]["Happy"].asBool(); + + face.facePointsInInfraredSpace.eyeLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["X"].asDouble(); + face.facePointsInInfraredSpace.eyeLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["Y"].asDouble(); + face.facePointsInInfraredSpace.eyeRightX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInInfraredSpace.eyeRightY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInInfraredSpace.noseX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInInfraredSpace.noseY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["X"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["Y"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerRightX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["X"].asDouble(); + face.facePointsInInfraredSpace.mouthCornerRightY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["Y"].asDouble(); + + face.facePointsInColorSpace.eyeLeftX = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["X"].asDouble(); + face.facePointsInColorSpace.eyeLeftY = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["Y"].asDouble(); + face.facePointsInColorSpace.eyeRightX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInColorSpace.eyeRightY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInColorSpace.noseX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"].asDouble(); + face.facePointsInColorSpace.noseY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"].asDouble(); + face.facePointsInColorSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["X"].asDouble(); + face.facePointsInColorSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["Y"].asDouble(); + face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"].asDouble(); + face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"].asDouble(); + + face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"].asInt(); + face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"].asInt(); + face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"].asInt(); + face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"].asInt(); + + face.faceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"].asInt(); + face.faceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"].asInt(); + face.faceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"].asInt(); + face.faceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"].asInt(); + + face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"].asDouble(); + face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"].asDouble(); + face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"].asDouble(); + face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"].asDouble(); + + //face.trackingId = jsonObject[i]["TrackingId"].asInt(); + face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"].asInt(); + + faceArray.faces.push_back(face); + } + } + catch (...) + { + ROS_ERROR("An exception occured"); + continue; + } + if (faceArray.faces.size() > 0) + facePub.publish(faceArray); + } + return 0; +} From 59bcf6c4b702edf04f26e041b2d6a70e92ed373d Mon Sep 17 00:00:00 2001 From: gjc13 Date: Tue, 17 May 2016 13:03:06 +0000 Subject: [PATCH 12/14] [Fix] Fix body joint position bug --- src/startBody.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/startBody.cpp b/src/startBody.cpp index c2d964a..8c8a603 100644 --- a/src/startBody.cpp +++ b/src/startBody.cpp @@ -123,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); From b30e161faf491b1613230b38cbfa5c1b2bb45d9c Mon Sep 17 00:00:00 2001 From: gjc13 Date: Sat, 25 Jun 2016 14:17:24 +0200 Subject: [PATCH 13/14] [Add] Send sound location from kinect --- src/startAudio.cpp | 35 ++++++++++------------------------- 1 file changed, 10 insertions(+), 25 deletions(-) diff --git a/src/startAudio.cpp b/src/startAudio.cpp index 86df3b4..4c2a92f 100644 --- a/src/startAudio.cpp +++ b/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 +#include +#include + 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(topicName,1); + Socket mySocket(serverAddress.c_str(),"9009",twiceStreamSize); + ros::Publisher audioPub = n.advertise(topicName,1); while(ros::ok()) { mySocket.readData(); - std::string jsonString; - for(int i=0;i Date: Fri, 1 Jul 2016 15:00:31 +0000 Subject: [PATCH 14/14] Robocup 2016 --- launch/k2_client.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/k2_client.launch b/launch/k2_client.launch index 2021bff..2590f21 100644 --- a/launch/k2_client.launch +++ b/launch/k2_client.launch @@ -9,7 +9,8 @@ - + +