36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_QUALISYS
43 #include <visp3/core/vpTime.h>
44 #include <visp3/sensor/vpMocapQualisys.h>
46 #include <qualisys_cpp_sdk/RTPacket.h>
47 #include <qualisys_cpp_sdk/RTProtocol.h>
49 #ifndef DOXYGEN_SHOULD_SKIP_THIS
50 class vpMocapQualisys::vpMocapQualisysImpl
54 : m_rtProtocol(), m_basePort(22222), m_udpPort(6734), m_majorVersion(1), m_minorVersion(19), m_bigEndian(false),
59 virtual ~vpMocapQualisysImpl() {
close(); }
63 m_rtProtocol.StopCapture();
64 m_rtProtocol.Disconnect();
70 for (
auto i = 0; i < n_attempt; i++) {
71 if (!m_rtProtocol.Connected()) {
72 if (!m_rtProtocol.Connect(
m_serverAddr.c_str(), m_basePort, &m_udpPort, m_majorVersion, m_minorVersion,
74 std::cout <<
"Qualisys connection error: " << m_rtProtocol.GetErrorString() << std::endl;
80 std::cout <<
"Qualisys connected" << std::endl;
82 return verifyDataStreamed();
86 std::cout <<
"Qualisys connection timeout" << std::endl;
91 bool verifyDataStreamed()
93 bool readSettingsOK =
false;
95 for (
auto i = 0; i < 6; i++) {
96 if (!m_dataAvailable) {
97 if (!m_rtProtocol.Read6DOFSettings(m_dataAvailable)) {
99 std::cout <<
"Reading 6DOF settings error: " << m_rtProtocol.GetErrorString() << std::endl;
106 std::cout <<
"Reading 6DOF settings succeded." << std::endl;
108 readSettingsOK =
true;
112 if (!readSettingsOK) {
114 std::cout <<
"Reading 6DOF settings timeout: " << std::endl;
118 for (
auto i = 0; i < 6; i++) {
119 if (!m_streamFrames) {
120 if (!m_rtProtocol.StreamFrames(CRTProtocol::RateAllFrames, 0, m_udpPort,
nullptr, CRTProtocol::cComponent6d)) {
122 std::cout <<
"Streaming frames error: " << m_rtProtocol.GetErrorString() << std::endl;
127 m_streamFrames =
true;
130 std::cout <<
"Starting to stream 6DOF data" << std::endl;
136 std::cout <<
"Streaming frames timeout: " << std::endl;
142 bool getBodyPose(
int iBody, std::string &name,
vpHomogeneousMatrix &M, CRTPacket *rtPacket)
145 float rotationMatrix[9];
147 if (rtPacket->Get6DOFBody(iBody, fX, fY, fZ, rotationMatrix)) {
148 const char *pTmpStr = m_rtProtocol.Get6DOFBodyName(iBody);
150 name = std::string(pTmpStr);
153 std::cout <<
"Unknown body" << std::endl;
158 M[0][3] = fX / 1000.;
159 M[1][3] = fY / 1000.;
160 M[2][3] = fZ / 1000.;
163 for (
unsigned int j = 0; j < 3; j++) {
164 for (
unsigned int i = 0; i < 3; i++) {
165 M[i][j] = rotationMatrix[k++];
175 bool getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose,
bool all_bodies)
177 CRTPacket::EPacketType packetType;
179 if (m_rtProtocol.Receive(packetType,
true) == CNetwork::ResponseType::success) {
180 if (packetType == CRTPacket::PacketData) {
181 CRTPacket *rtPacket = m_rtProtocol.GetRTPacket();
182 for (
unsigned int iBody = 0; iBody < rtPacket->Get6DOFBodyCount(); iBody++) {
183 std::string bodyName;
186 if (!getBodyPose(iBody, bodyName, bodyPose, rtPacket)) {
187 std::cout <<
"Error : Could not get pose from body n°" << iBody << std::endl;
192 bodies_pose[bodyName] = bodyPose;
193 }
else if (bodyPose.
isValid()) {
194 bodies_pose[bodyName] = bodyPose;
205 std::map<std::string, vpHomogeneousMatrix> bodies_pose;
207 if (bodies_pose.find(body_name) != bodies_pose.end()) {
208 body_pose = bodies_pose[body_name];
210 std::cout <<
"I found bodyName" << body_name << std::endl;
214 std::cout <<
"The body " << body_name <<
" was not found in Qualisys. Please check the name you typed."
220 std::cout <<
"Error : could not process data from Qualisys" << std::endl;
231 CRTProtocol m_rtProtocol;
232 unsigned short m_basePort;
233 unsigned short m_udpPort;
237 bool m_dataAvailable;
279 return m_impl->getBodiesPose(bodies_pose, all_bodies);
290 return m_impl->getSpecificBodyPose(body_name, body_pose);
309 void dummy_vpMocapQualisys(){};
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setServerAddress(const std::string &serverAddr)
bool getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
void setVerbose(bool verbose)
virtual ~vpMocapQualisys()
bool getBodiesPose(std::map< std::string, vpHomogeneousMatrix > &bodies_pose, bool all_bodies=false)
VISP_EXPORT void sleepMs(double t)