Skip to content

Commit fe03a80

Browse files
progtologistprogtologist
committed
Fixed errors in body communication.
1 parent 9780df7 commit fe03a80

File tree

1 file changed

+119
-102
lines changed

1 file changed

+119
-102
lines changed

‎src/startBody.cpp

+119-102
Original file line numberDiff line numberDiff line change
@@ -29,125 +29,142 @@ WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
2929
#include <iconv.h>
3030

3131
std::string topicName = "bodyArray";
32-
size_t streamSize = 56008;
33-
size_t readSkipSize = 56000;
34-
size_t stringSize = 28000;
32+
int readSkipSize = 56000;
33+
int stringSize = 28000;
34+
int streamSize = readSkipSize + sizeof(double);
3535

3636
int main(int argC,char **argV)
3737
{
3838
ros::init(argC,argV,"startBody");
3939
ros::NodeHandle n;
4040
std::string serverAddress;
4141
n.getParam("/serverNameOrIP",serverAddress);
42-
Socket mySocket(serverAddress.c_str(),const_cast<char*>("9003"),streamSize);
43-
iconv_t charConverter = iconv_open("UTF-8","UTF-16");
42+
Socket mySocket(serverAddress.c_str(),const_cast<char*>("9003"), streamSize);
4443
ros::Publisher bodyPub = n.advertise<k2_client::BodyArray>(topicName,1);
45-
char jsonCharArray[readSkipSize];
44+
char utf16Array[readSkipSize];
45+
char jsonCharArray[stringSize];
46+
char *jsonCharArrayPtr;
47+
char *utf16ArrayPtr;
48+
ros::Rate r(30);
4649
while(ros::ok())
4750
{
4851
mySocket.readData();
49-
char *jsonCharArrayPtr;
50-
char *socketCharArrayPtr;
51-
jsonCharArrayPtr = jsonCharArray;
52-
socketCharArrayPtr = mySocket.mBuffer;
53-
iconv(charConverter,&socketCharArrayPtr,&readSkipSize,&jsonCharArrayPtr,&stringSize);
54-
double utcTime;
55-
memcpy(&utcTime,&mySocket.mBuffer[readSkipSize],sizeof(double));
52+
jsonCharArrayPtr = jsonCharArray;
53+
utf16ArrayPtr = utf16Array;
54+
memset(utf16Array, 0, sizeof(utf16Array));
55+
memcpy(utf16Array, mySocket.mBuffer, sizeof(utf16Array));
56+
size_t iconv_in = static_cast<size_t>(readSkipSize);
57+
size_t iconv_out = static_cast<size_t>(stringSize);
58+
iconv_t charConverter = iconv_open("UTF-8","UTF-16");
59+
size_t nconv = iconv(charConverter, &utf16ArrayPtr, &iconv_in, &jsonCharArrayPtr, &iconv_out);
60+
iconv_close(charConverter);
61+
if (nconv == (size_t) - 1)
62+
{
63+
if (errno == EINVAL)
64+
ROS_ERROR("An incomplete multibyte sequence has been encountered in the input.");
65+
else if (errno == EILSEQ)
66+
ROS_ERROR("An invalid multibyte sequence has been encountered in the input.");
67+
else
68+
ROS_ERROR("There is not sufficient room at jsonCharArray");
69+
}
70+
double utcTime = 0.0;
71+
memcpy(&utcTime,&mySocket.mBuffer[readSkipSize], sizeof(double));
5672
std::string jsonString(jsonCharArray);
57-
//std::cout<<jsonCharArray<<std::endl<<"***"<<std::endl;
5873
Json::Value jsonObject;
5974
Json::Reader jsonReader;
60-
bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
61-
if(!parsingSuccessful)
62-
{
63-
std::cout<<"Failure to parse: "<<parsingSuccessful<<std::endl;
64-
continue;
65-
}
66-
k2_client::BodyArray bodyArray;
67-
try
68-
{
69-
for(int i=0;i<6;i++)
70-
{
71-
k2_client::Body body;
72-
body.header.stamp = ros::Time(utcTime);
73-
body.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
74-
body.leanTrackingState = jsonObject[i]["LeanTrackingState"].asInt();
75-
body.lean.leanX = jsonObject[i]["Lean"]["X"].asDouble();
76-
body.lean.leanY = jsonObject[i]["Lean"]["Y"].asDouble();
77-
body.isTracked = jsonObject[i]["IsTracked"].asBool();
78-
body.trackingId = jsonObject[i]["TrackingId"].asUInt64();
79-
body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt();
80-
body.engaged = jsonObject[i]["Engaged"].asBool();
81-
body.handRightConfidence = jsonObject[i]["HandRightConfidence"].asInt();
82-
body.handRightState = jsonObject[i]["HandRightState"].asInt();
83-
body.handLeftConfidence = jsonObject[i]["HandLeftConfidence"].asInt();
84-
body.handLeftState = jsonObject[i]["HandLeftState"].asInt();
85-
body.appearance.wearingGlasses = jsonObject[i]["Appearance"]["WearingGlasses"].asBool();
86-
body.activities.eyeLeftClosed = jsonObject[i]["Activities"]["EyeLeftClosed"].asBool();
87-
body.activities.eyeRightClosed = jsonObject[i]["Activities"]["EyeRightClosed"].asBool();
88-
body.activities.mouthOpen = jsonObject[i]["Activities"]["MouthOpen"].asBool();
89-
body.activities.mouthMoved = jsonObject[i]["Activities"]["MouthMoved"].asBool();
90-
body.activities.lookingAway = jsonObject[i]["Activities"]["LookingAway"].asBool();
91-
body.expressions.neutral = jsonObject[i]["Expressions"]["Neutral"].asBool();
92-
body.expressions.neutral = jsonObject[i]["Expressions"]["Happy"].asBool();
93-
for(int j=0;j<25;j++)
94-
{
95-
k2_client::JointOrientationAndType JOAT;
96-
k2_client::JointPositionAndState JPAS;
97-
std::string fieldName;
98-
switch (j)
99-
{
100-
case 0: fieldName = "SpineBase";break;
101-
case 1: fieldName = "SpineMid";break;
102-
case 2: fieldName = "Neck";break;
103-
case 3: fieldName = "Head";break;
104-
case 4: fieldName = "ShoulderLeft";break;
105-
case 5: fieldName = "ElbowLeft";break;
106-
case 6: fieldName = "WristLeft";break;
107-
case 7: fieldName = "HandLeft";break;
108-
case 8: fieldName = "ShoulderRight";break;
109-
case 9: fieldName = "ElbowRight";break;
110-
case 10: fieldName = "WristRight";break;
111-
case 11: fieldName = "HandRight";break;
112-
case 12: fieldName = "HipLeft";break;
113-
case 13: fieldName = "KneeLeft";break;
114-
case 14: fieldName = "AnkleLeft";break;
115-
case 15: fieldName = "SpineBase";break;
116-
case 16: fieldName = "HipRight";break;
117-
case 17: fieldName = "KneeRight";break;
118-
case 18: fieldName = "AnkleRight";break;
119-
case 19: fieldName = "FootRight";break;
120-
case 20: fieldName = "SpineShoulder";break;
121-
case 21: fieldName = "HandTipLeft";break;
122-
case 22: fieldName = "ThumbLeft";break;
123-
case 23: fieldName = "HandTipRight";break;
124-
case 24: fieldName = "ThumbRight";break;
125-
}
75+
bool parsingSuccessful = jsonReader.parse(jsonString,jsonObject,false);
76+
if(!parsingSuccessful)
77+
{
78+
ROS_ERROR("Failure to parse");
79+
continue;
80+
}
81+
k2_client::BodyArray bodyArray;
82+
try
83+
{
84+
for(int i=0;i<6;i++)
85+
{
86+
k2_client::Body body;
87+
body.header.stamp = ros::Time(utcTime);
88+
body.header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/depthFrame";
89+
body.leanTrackingState = jsonObject[i]["LeanTrackingState"].asInt();
90+
body.lean.leanX = jsonObject[i]["Lean"]["X"].asDouble();
91+
body.lean.leanY = jsonObject[i]["Lean"]["Y"].asDouble();
92+
body.isTracked = jsonObject[i]["IsTracked"].asBool();
93+
body.trackingId = jsonObject[i]["TrackingId"].asUInt64();
94+
body.clippedEdges = jsonObject[i]["ClippedEdges"].asInt();
95+
body.engaged = jsonObject[i]["Engaged"].asBool();
96+
body.handRightConfidence = jsonObject[i]["HandRightConfidence"].asInt();
97+
body.handRightState = jsonObject[i]["HandRightState"].asInt();
98+
body.handLeftConfidence = jsonObject[i]["HandLeftConfidence"].asInt();
99+
body.handLeftState = jsonObject[i]["HandLeftState"].asInt();
100+
body.appearance.wearingGlasses = jsonObject[i]["Appearance"]["WearingGlasses"].asBool();
101+
body.activities.eyeLeftClosed = jsonObject[i]["Activities"]["EyeLeftClosed"].asBool();
102+
body.activities.eyeRightClosed = jsonObject[i]["Activities"]["EyeRightClosed"].asBool();
103+
body.activities.mouthOpen = jsonObject[i]["Activities"]["MouthOpen"].asBool();
104+
body.activities.mouthMoved = jsonObject[i]["Activities"]["MouthMoved"].asBool();
105+
body.activities.lookingAway = jsonObject[i]["Activities"]["LookingAway"].asBool();
106+
body.expressions.neutral = jsonObject[i]["Expressions"]["Neutral"].asBool();
107+
body.expressions.neutral = jsonObject[i]["Expressions"]["Happy"].asBool();
108+
for(int j=0;j<25;j++)
109+
{
110+
k2_client::JointOrientationAndType JOAT;
111+
k2_client::JointPositionAndState JPAS;
112+
std::string fieldName;
113+
switch (j)
114+
{
115+
case 0: fieldName = "SpineBase";break;
116+
case 1: fieldName = "SpineMid";break;
117+
case 2: fieldName = "Neck";break;
118+
case 3: fieldName = "Head";break;
119+
case 4: fieldName = "ShoulderLeft";break;
120+
case 5: fieldName = "ElbowLeft";break;
121+
case 6: fieldName = "WristLeft";break;
122+
case 7: fieldName = "HandLeft";break;
123+
case 8: fieldName = "ShoulderRight";break;
124+
case 9: fieldName = "ElbowRight";break;
125+
case 10: fieldName = "WristRight";break;
126+
case 11: fieldName = "HandRight";break;
127+
case 12: fieldName = "HipLeft";break;
128+
case 13: fieldName = "KneeLeft";break;
129+
case 14: fieldName = "AnkleLeft";break;
130+
case 15: fieldName = "SpineBase";break;
131+
case 16: fieldName = "HipRight";break;
132+
case 17: fieldName = "KneeRight";break;
133+
case 18: fieldName = "AnkleRight";break;
134+
case 19: fieldName = "FootRight";break;
135+
case 20: fieldName = "SpineShoulder";break;
136+
case 21: fieldName = "HandTipLeft";break;
137+
case 22: fieldName = "ThumbLeft";break;
138+
case 23: fieldName = "HandTipRight";break;
139+
case 24: fieldName = "ThumbRight";break;
140+
}
126141

127-
JOAT.orientation.x = jsonObject[i][fieldName]["Orientation"]["X"].asDouble();
128-
JOAT.orientation.y = jsonObject[i][fieldName]["Orientation"]["Y"].asDouble();
129-
JOAT.orientation.z = jsonObject[i][fieldName]["Orientation"]["Z"].asDouble();
130-
JOAT.orientation.w = jsonObject[i][fieldName]["Orientation"]["W"].asDouble();
131-
JOAT.jointType = jsonObject[i][fieldName]["JointType"].asInt();
142+
JOAT.orientation.x = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["X"].asDouble();
143+
JOAT.orientation.y = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Y"].asDouble();
144+
JOAT.orientation.z = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["Z"].asDouble();
145+
JOAT.orientation.w = jsonObject[i]["JointOrientations"][fieldName]["Orientation"]["W"].asDouble();
146+
JOAT.jointType = jsonObject[i]["JointOrientations"][fieldName]["JointType"].asInt();
132147

133-
JPAS.trackingState = jsonObject[i][fieldName]["TrackingState"].asBool();
134-
JPAS.position.x = jsonObject[i][fieldName]["Position"]["X"].asDouble();
135-
JPAS.position.y = jsonObject[i][fieldName]["Position"]["Y"].asDouble();
136-
JPAS.position.z = jsonObject[i][fieldName]["Position"]["Z"].asDouble();
137-
JPAS.jointType = jsonObject[i][fieldName]["JointType"].asInt();
148+
JPAS.trackingState = jsonObject[i]["Joints"][fieldName]["TrackingState"].asBool();
149+
JPAS.position.x = jsonObject[i]["Joints"][fieldName]["Position"]["X"].asDouble();
150+
JPAS.position.y = jsonObject[i]["Joints"][fieldName]["Position"]["Y"].asDouble();
151+
JPAS.position.z = jsonObject[i]["Joints"][fieldName]["Position"]["Z"].asDouble();
152+
JPAS.jointType = jsonObject[i]["Joints"][fieldName]["JointType"].asInt();
138153

139-
body.jointOrientations.push_back(JOAT);
140-
body.jointPositions.push_back(JPAS);
141-
}
142-
bodyArray.bodies.push_back(body);
143-
}
144-
}
145-
catch (...)
146-
{
147-
std::cout<<"An exception occured"<<std::endl;
148-
continue;
149-
}
150-
bodyPub.publish(bodyArray);
154+
body.jointOrientations.push_back(JOAT);
155+
body.jointPositions.push_back(JPAS);
156+
}
157+
bodyArray.bodies.push_back(body);
158+
}
159+
}
160+
catch (...)
161+
{
162+
ROS_ERROR("An exception occured");
163+
continue;
164+
}
165+
bodyPub.publish(bodyArray);
166+
ros::spinOnce();
167+
r.sleep();
151168
}
152169
return 0;
153170
}

0 commit comments

Comments
 (0)