Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMocapVicon.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Motion capture using Vicon device.
33 *
34*****************************************************************************/
35
36#include <visp3/core/vpConfig.h>
37
38#ifdef VISP_HAVE_VICON
39
40#include <cmath>
41#include <iostream>
42
43#include <visp3/core/vpTime.h>
44#include <visp3/sensor/vpMocapVicon.h>
45
46#include <DataStreamClient.h>
47#include <IDataStreamClientBase.h>
48
49using namespace ViconDataStreamSDK::CPP;
50
51#ifndef DOXYGEN_SHOULD_SKIP_THIS
52class vpMocapVicon::vpMocapViconImpl
53{
54public:
55 vpMocapViconImpl() : m_DirectClient(), m_verbose(false), m_serverAddr() {}
56 virtual ~vpMocapViconImpl() { close(); }
57
58 void close()
59 {
60 m_DirectClient.DisableSegmentData();
61 m_DirectClient.DisableMarkerData();
62 m_DirectClient.DisableUnlabeledMarkerData();
63 m_DirectClient.DisableDeviceData();
64 if (m_verbose) {
65 std::cout << "Disconnecting..." << std::endl;
66 }
67 m_DirectClient.Disconnect();
68 }
69
70 bool connect()
71 {
72 int n_attempt = 2;
73 for (auto i = 0; i < n_attempt; i++) {
74 if (!m_DirectClient.IsConnected().Connected) {
75 // Direct connection
76
77 const Output_Connect ConnectResult = m_DirectClient.Connect(m_serverAddr);
78 const bool ok = (ConnectResult.Result == Result::Success);
79
80 if (!ok) {
81 if (m_verbose) {
82 std::cout << "Warning - connection failed... ";
83 switch (ConnectResult.Result) {
84 case Result::ClientAlreadyConnected:
85 std::cout << "Client Already Connected" << std::endl;
86 break;
87 case Result::InvalidHostName:
88 std::cout << "Invalid Host Name" << std::endl;
89 break;
90 case Result::ClientConnectionFailed:
91 std::cout << "Client Connection Failed" << std::endl;
92 break;
93 default:
94 std::cout << "Unrecognized Error: " << ConnectResult.Result << std::endl;
95 break;
96 }
97 }
98 vpTime::sleepMs(1000);
99 }
100 if (ok) {
101 if (m_verbose) {
102 std::cout << "Successful connection to : " << m_serverAddr << std::endl;
103 }
104 return setupDataStreamed();
105 }
106 }
107 }
108
109 if (m_verbose) {
110 std::cout << "Vicon connection timeout" << std::endl;
111 }
112 return false;
113 }
114
115 bool getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies = false)
116 {
117 if (m_DirectClient.GetFrame().Result == Result::Success) {
118 for (unsigned int iBody = 0; iBody < m_DirectClient.GetSubjectCount().SubjectCount; iBody++) {
119 std::string bodyName = m_DirectClient.GetSubjectName(iBody).SubjectName;
120 std::string rootSegment = m_DirectClient.GetSubjectRootSegmentName(bodyName).SegmentName;
121 bool data_extraction_success = (m_DirectClient.GetSegmentGlobalRotationMatrix(bodyName, rootSegment).Result &&
122 m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Result);
123 vpHomogeneousMatrix bodyPose;
124
125 if (!data_extraction_success) {
126 std::cout << "Error : Could not get pose from body n°" << iBody << std::endl;
127
128 return false;
129 } else {
130 bodyPose[0][3] = m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Translation[0] / 1000.0;
131 bodyPose[1][3] = m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Translation[1] / 1000.0;
132 bodyPose[2][3] = m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Translation[2] / 1000.0;
133 bodyPose[3][3] = 1.0;
134
135 // Vicon is row major
136 unsigned int k = 0;
137 for (unsigned int i = 0; i < 3; i++) {
138 for (unsigned int j = 0; j < 3; j++) {
139 bodyPose[i][j] = m_DirectClient.GetSegmentGlobalRotationMatrix(bodyName, rootSegment).Rotation[k++];
140 }
141 }
142 }
143 if (all_bodies) {
144 bodies_pose[bodyName] = bodyPose;
145 } else if (bodyPose.isValid()) {
146 bodies_pose[bodyName] = bodyPose;
147 }
148 }
149 return true;
150 }
151 return false;
152 }
153
154 bool getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
155 {
156 std::map<std::string, vpHomogeneousMatrix> bodies_pose;
157 if (getBodiesPose(bodies_pose, true)) {
158 if (bodies_pose.find(body_name) != bodies_pose.end()) {
159 body_pose = bodies_pose[body_name];
160 return true;
161 } else {
162 std::cout << "The body " << body_name << " was not found in Vicon. Please check the name you typed."
163 << std::endl;
164
165 return false;
166 }
167 } else {
168 std::cout << "Error : could not process data from Vicon" << std::endl;
169
170 return false;
171 }
172 }
173
174 void setServerAddress(const std::string &serverAddr) { m_serverAddr = serverAddr; }
175
176 void setVerbose(bool verbose) { m_verbose = verbose; }
177
178 bool setupDataStreamed()
179 {
180 // We set up the kind of data we get.
181 m_DirectClient.EnableSegmentData();
182 m_DirectClient.EnableMarkerData();
183 m_DirectClient.EnableUnlabeledMarkerData();
184 m_DirectClient.EnableMarkerRayData();
185 m_DirectClient.EnableDeviceData();
186 m_DirectClient.EnableDebugData();
187 // We set up which kind of connectiion we want.
188 m_DirectClient.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
189
190 // Set the global up axis
191 m_DirectClient.SetAxisMapping(Direction::Forward, Direction::Left,
192 Direction::Up); // Z-up
193
194 return true;
195 }
196
197private:
198 ViconDataStreamSDK::CPP::Client m_DirectClient;
199 bool m_verbose;
200 std::string m_serverAddr;
201};
202#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
203
204/*
205 **********************************************************************************************
206 */
207
211vpMocapVicon::vpMocapVicon() : m_impl(new vpMocapViconImpl()) {}
212
216vpMocapVicon::~vpMocapVicon() { delete m_impl; }
217
221void vpMocapVicon::close() { m_impl->close(); }
222
228bool vpMocapVicon::connect() { return m_impl->connect(); }
229
237bool vpMocapVicon::getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies)
238{
239 return m_impl->getBodiesPose(bodies_pose, all_bodies);
240}
241
248bool vpMocapVicon::getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
249{
250 return m_impl->getSpecificBodyPose(body_name, body_pose);
251}
252
259void vpMocapVicon::setServerAddress(const std::string &serverAddr) { m_impl->setServerAddress(serverAddr); }
260
265void vpMocapVicon::setVerbose(bool verbose) { m_impl->setVerbose(verbose); }
266
267#else
268// Work around to avoid warning:
269// libvisp_sensor.a(vpMocapVicon.cpp.o) has no symbols
270void dummy_vpMocapVicon(){};
271#endif
Implementation of an homogeneous matrix and operations on such kind of matrices.
bool getBodiesPose(std::map< std::string, vpHomogeneousMatrix > &bodies_pose, bool all_bodies=false)
void setVerbose(bool verbose)
virtual ~vpMocapVicon()
void setServerAddress(const std::string &serverAddr)
bool getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
VISP_EXPORT void sleepMs(double t)