Skip to content

Commit cf94c56

Browse files
sugar10wsugar10w
committed
[Add][Debugging] startFace
1 parent c20f1eb commit cf94c56

File tree

8 files changed

+147
-1
lines changed

8 files changed

+147
-1
lines changed

‎CMakeLists.txt‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,11 +26,16 @@ add_message_files(FILES
2626
Appearance.msg
2727
Audio.msg
2828
BodyArray.msg
29+
Body.msg
30+
BoundingBox2D.msg
2931
Expressions.msg
32+
FaceArray.msg
33+
Face.msg
34+
FacePoints.msg
3035
JointOrientationAndType.msg
3136
JointPositionAndState.msg
3237
Lean.msg
33-
Body.msg
38+
Quaternion.msg
3439
)
3540

3641
generate_messages(DEPENDENCIES
@@ -70,18 +75,22 @@ add_executable(startBody "${PROJECT_SOURCE_DIR}/src/startBody.cpp")
7075
add_executable(startDepth "${PROJECT_SOURCE_DIR}/src/startDepth.cpp")
7176
add_executable(startIR "${PROJECT_SOURCE_DIR}/src/startIR.cpp")
7277
add_executable(startRGB "${PROJECT_SOURCE_DIR}/src/startRGB.cpp")
78+
add_executable(startFace "${PROJECT_SOURCE_DIR}/src/startFace.cpp")
7379

7480
add_dependencies(startAudio ${k2_client_EXPORTED_TARGETS})
7581
add_dependencies(startBody ${k2_client_EXPORTED_TARGETS})
7682
add_dependencies(startDepth ${k2_client_EXPORTED_TARGETS})
7783
add_dependencies(startIR ${k2_client_EXPORTED_TARGETS})
7884
add_dependencies(startRGB ${k2_client_EXPORTED_TARGETS})
85+
add_dependencies(startFace ${k2_client_EXPORTED_TARGETS})
7986

8087
target_link_libraries(startAudio ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
8188
target_link_libraries(startBody ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
8289
target_link_libraries(startDepth ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
8390
target_link_libraries(startIR ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
8491
target_link_libraries(startRGB ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
92+
target_link_libraries(startFace ${JSONCPP_LIBRARY} ${catkin_LIBRARIES})
93+
8594

8695

8796

‎launch/k2_client.launch‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,5 +9,6 @@
99
<node machine="hyperv-linux" name="startRGB" pkg="k2_client" type="startRGB"/>
1010
<node machine="hyperv-linux" name="startDepth" pkg="k2_client" type="startDepth" output="screen"/>
1111
<node machine="hyperv-linux" name="startBody" pkg="k2_client" type="startBody" output="screen"/>
12+
<node machine="hyperv-linux" name="startFace" pkg="k2_client" type="startFace" output="screen"/>
1213
</group>
1314
</launch>

‎msg/BoundingBox2D.msg‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
int32 left
2+
int32 top
3+
int32 right
4+
int32 bottom

‎msg/Face.msg‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
Header header
2+
3+
Appearance appearance
4+
Activities activities
5+
Expressions expressions
6+
7+
FacePoints facePointsInInfraredSpace
8+
FacePoints facePointsInColorSpace
9+
10+
BoundingBox2D faceBoundingBoxInInfraredSpace
11+
BoundingBox2D FaceBoundingBoxInColorSpace
12+
13+
Quaternion faceRotationQuaternion
14+
15+
int64 trackingId
16+
int32 faceFrameFeatures

‎msg/FaceArray.msg‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
Face[] faces

‎msg/FacePoints.msg‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
float32 eyeLeftX
2+
float32 eyeLeftY
3+
float32 eyeRightX
4+
float32 eyeRightY
5+
float32 noseX
6+
float32 noseY
7+
float32 mouthCornerLeftX
8+
float32 mouthCornerLeftY
9+
float32 mouthCornerRightX
10+
float32 mouthCornerRightY

‎msg/Quaternion.msg‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
float32 X
2+
float32 Y
3+
float32 Z
4+
float32 W

‎src/startFace.cpp‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
#include "k2_client/k2_client.h"
2+
#include "k2_client/FaceArray.h"
3+
#include <iconv.h>
4+
#include <cstdio>
5+
6+
std::string topicName = "faceArray";
7+
size_t streamSize = 60000;
8+
size_t readSkipSize = 60000;
9+
size_t stringSize = 30000;
10+
11+
int main(int argC,char **argV)
12+
{
13+
ros::init(argC,argV,"startFace");
14+
ros::NodeHandle n;
15+
std::string serverAddress;
16+
n.getParam("/serverNameOrIP",serverAddress);
17+
Socket mySocket(serverAddress.c_str(),"9006",streamSize);
18+
ros::Publisher facePub = n.advertise<k2_client::FaceArray>(topicName,1);
19+
while(ros::ok())
20+
{
21+
mySocket.readData();
22+
std::string jsonString(mySocket.mBuffer);
23+
Json::Value jsonObject;
24+
Json::Reader jsonReader;
25+
bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
26+
if(!parsingSuccessful)
27+
{
28+
ROS_ERROR("Failure to parse");
29+
continue;
30+
}
31+
k2_client::FaceArray faceArray;
32+
try
33+
{
34+
for(int i=0;i<6;i++)
35+
{
36+
k2_client::Face face;
37+
face.header.stamp = ros::Time::now();
38+
face.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
39+
40+
face.appearance.wearingGlasses = jsonObject[i]["FaceProperties"]["WearingGlasses"].asBool();
41+
face.activities.eyeLeftClosed = jsonObject[i]["FaceProperties"]["EyeLeftClosed"].asBool();
42+
face.activities.eyeRightClosed = jsonObject[i]["FaceProperties"]["EyeRightClosed"].asBool();
43+
face.activities.mouthOpen = jsonObject[i]["FaceProperties"]["MouthOpen"].asBool();
44+
face.activities.mouthMoved = jsonObject[i]["FaceProperties"]["MouthMoved"].asBool();
45+
face.activities.lookingAway = jsonObject[i]["FaceProperties"]["LookingAway"].asBool();
46+
face.expressions.neutral = jsonObject[i]["FaceProperties"]["Neutral"].asBool();
47+
face.expressions.neutral = jsonObject[i]["FaceProperties"]["Happy"].asBool();
48+
49+
face.facePointsInInfraredSpace.eyeLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["X"];
50+
face.facePointsInInfraredSpace.eyeLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeLeft"]["Y"];
51+
face.facePointsInInfraredSpace.eyeRightX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"];
52+
face.facePointsInInfraredSpace.eyeRightY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"];
53+
face.facePointsInInfraredSpace.noseX = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["X"];
54+
face.facePointsInInfraredSpace.noseY = jsonObject[i]["FacePointsInInfraredSpace"]["EyeRight"]["Y"];
55+
face.facePointsInInfraredSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["X"];
56+
face.facePointsInInfraredSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerLeft"]["Y"];
57+
face.facePointsInInfraredSpace.mouthCornerRightX = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["X"];
58+
face.facePointsInInfraredSpace.mouthCornerRightY = jsonObject[i]["FacePointsInInfraredSpace"]["MouthCornerRight"]["Y"];
59+
60+
face.facePointsInColorSpace.eyeLeftX = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["X"];
61+
face.facePointsInColorSpace.eyeLeftY = jsonObject[i]["FacePointsInColorSpace"]["EyeLeft"]["Y"];
62+
face.facePointsInColorSpace.eyeRightX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"];
63+
face.facePointsInColorSpace.eyeRightY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"];
64+
face.facePointsInColorSpace.noseX = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["X"];
65+
face.facePointsInColorSpace.noseY = jsonObject[i]["FacePointsInColorSpace"]["EyeRight"]["Y"];
66+
face.facePointsInColorSpace.mouthCornerLeftX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["X"];
67+
face.facePointsInColorSpace.mouthCornerLeftY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerLeft"]["Y"];
68+
face.facePointsInColorSpace.mouthCornerRightX = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["X"];
69+
face.facePointsInColorSpace.mouthCornerRightY = jsonObject[i]["FacePointsInColorSpace"]["MouthCornerRight"]["Y"];
70+
71+
face.faceBoundingBoxInInfraredSpace.left = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Left"];
72+
face.faceBoundingBoxInInfraredSpace.top = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Top"];
73+
face.faceBoundingBoxInInfraredSpace.right = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Right"];
74+
face.faceBoundingBoxInInfraredSpace.bottom = jsonObject[i]["FaceBoundingBoxInInfraredSpace"]["Bottom"];
75+
76+
face.FaceBoundingBoxInColorSpace.left = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Left"];
77+
face.FaceBoundingBoxInColorSpace.top = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Top"];
78+
face.FaceBoundingBoxInColorSpace.right = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Right"];
79+
face.FaceBoundingBoxInColorSpace.bottom = jsonObject[i]["FaceBoundingBoxInColorSpace"]["Bottom"];
80+
81+
face.faceRotationQuaternion.X = jsonObject[i]["FaceRotationQuaternion"]["X"];
82+
face.faceRotationQuaternion.Y = jsonObject[i]["FaceRotationQuaternion"]["Y"];
83+
face.faceRotationQuaternion.Z = jsonObject[i]["FaceRotationQuaternion"]["Z"];
84+
face.faceRotationQuaternion.W = jsonObject[i]["FaceRotationQuaternion"]["W"];
85+
86+
face.trackingId = jsonObject[i]["TrackingId"];
87+
face.faceFrameFeatures = jsonObject[i]["FaceFrameFeatures"];
88+
89+
faceArray.faces.push_back(face);
90+
}
91+
}
92+
catch (...)
93+
{
94+
ROS_ERROR("An exception occured");
95+
continue;
96+
}
97+
if (faceArray.faces.size() > 0)
98+
facePub.publish(faceArray);
99+
}
100+
return 0;
101+
}

0 commit comments

Comments
 (0)