forked from vislab-tecnico-lisboa/KinectBody2Yarp
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Body.h
155 lines (135 loc) · 4.78 KB
/
Body.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
// This is an automatically generated file.
// Generated from this Body.msg definition:
// # A tracked person
//
// # The person's track ID, usually very large numbers, converted to string just because historical reasons
// string trackID
//
// # The 25 joints to be tracked that compose a person
// JointROSmsg[25] joints// Instances of this class can be read and written with YARP ports,
// using a ROS-compatible format.
#ifndef YARPMSG_TYPE_Body
#define YARPMSG_TYPE_Body
#include <string>
#include <vector>
#include <yarp/os/Wire.h>
#include <yarp/os/idl/WireTypes.h>
#include "FloorPlane.h"
#include "geometry_msgs_Point.h"
#include "geometry_msgs_Quaternion.h"
#include "geometry_msgs_Pose.h"
#include "JointROSmsg.h"
class Body : public yarp::os::idl::WirePortable {
public:
std::string trackID;
std::vector<JointROSmsg> joints;
Body() {
joints.resize(25);
}
bool readBare(yarp::os::ConnectionReader& connection) {
// *** trackID ***
int len = connection.expectInt();
trackID.resize(len);
if (!connection.expectBlock((char*)trackID.c_str(),len)) return false;
// *** joints ***
len = connection.expectInt();
joints.resize(len);
for (int i=0; i<len; i++) {
if (!joints[i].read(connection)) return false;
}
return !connection.isError();
}
bool readBottle(yarp::os::ConnectionReader& connection) {
connection.convertTextMode();
yarp::os::idl::WireReader reader(connection);
if (!reader.readListHeader(2)) return false;
// *** trackID ***
if (!reader.readString(trackID)) return false;
// *** joints ***
if (connection.expectInt()!=BOTTLE_TAG_LIST) return false;
int len = connection.expectInt();
joints.resize(len);
for (int i=0; i<len; i++) {
if (!joints[i].read(connection)) return false;
}
return !connection.isError();
}
bool read(yarp::os::ConnectionReader& connection) {
if (connection.isBareMode()) return readBare(connection);
return readBottle(connection);
}
bool writeBare(yarp::os::ConnectionWriter& connection) {
// *** trackID ***
connection.appendInt(trackID.length());
connection.appendExternalBlock((char*)trackID.c_str(),trackID.length());
// *** joints ***
for (size_t i=0; i<joints.size(); i++) {
if (!joints[i].write(connection)) return false;
}
return !connection.isError();
}
bool writeBottle(yarp::os::ConnectionWriter& connection) {
connection.appendInt(BOTTLE_TAG_LIST);
connection.appendInt(2);
// *** trackID ***
connection.appendInt(BOTTLE_TAG_STRING);
connection.appendInt(trackID.length());
connection.appendExternalBlock((char*)trackID.c_str(),trackID.length());
// *** joints ***
connection.appendInt(BOTTLE_TAG_LIST);
connection.appendInt(joints.size());
for (size_t i=0; i<joints.size(); i++) {
if (!joints[i].write(connection)) return false;
}
connection.convertTextMode();
return !connection.isError();
}
bool write(yarp::os::ConnectionWriter& connection) {
if (connection.isBareMode()) return writeBare(connection);
return writeBottle(connection);
}
// This class will serialize ROS style or YARP style depending on protocol.
// If you need to force a serialization style, use one of these classes:
typedef yarp::os::idl::BareStyle<Body> rosStyle;
typedef yarp::os::idl::BottleStyle<Body> bottleStyle;
// Give source text for class, ROS will need this
yarp::os::ConstString getTypeText() {
return "# A tracked person\n\
\n\
# The person's track ID, usually very large numbers, converted to string just because historical reasons\n\
string trackID\n\
\n\
# The 25 joints to be tracked that compose a person\n\
JointROSmsg[25] joints\n================================================================================\n\
MSG: JointROSmsg\n\
# A body joint that is tracked by kinect\n\
\n\
# The joint number and description\n\
int64 typeNum\n\
string typeStr\n\
\n\
# Position in space and orientation\n\
geometry_msgs/Pose pose\n\
\n================================================================================\n\
MSG: geometry_msgs/Pose\n\
geometry_msgs/Point position\n\
geometry_msgs/Quaternion orientation\n================================================================================\n\
MSG: geometry_msgs/Point\n\
float64 x\n\
float64 y\n\
float64 z\n================================================================================\n\
MSG: geometry_msgs/Quaternion\n\
float64 x\n\
float64 y\n\
float64 z\n\
float64 w";
}
// Name the class, ROS will need this
yarp::os::Type getType() {
yarp::os::Type typ = yarp::os::Type::byName("Body","Body");
typ.addProperty("md5sum",yarp::os::Value("4ad73cf7542a93600ae50d374a707ece"));
typ.addProperty("message_definition",yarp::os::Value(getTypeText()));
return typ;
}
};
#endif