Visual Servoing Platform  version 3.5.1 under development (2023-02-02)
vpMocapVicon.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 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 http://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 
49 using namespace ViconDataStreamSDK::CPP;
50 
51 #ifndef DOXYGEN_SHOULD_SKIP_THIS
52 class vpMocapVicon::vpMocapViconImpl
53 {
54 public:
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 
197 private:
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 
211 vpMocapVicon::vpMocapVicon() : m_impl(new vpMocapViconImpl()) {}
212 
216 vpMocapVicon::~vpMocapVicon() { delete m_impl; }
217 
221 void vpMocapVicon::close() { m_impl->close(); }
222 
228 bool vpMocapVicon::connect() { return m_impl->connect(); }
229 
237 bool vpMocapVicon::getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies)
238 {
239  return m_impl->getBodiesPose(bodies_pose, all_bodies);
240 }
241 
248 bool vpMocapVicon::getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
249 {
250  return m_impl->getSpecificBodyPose(body_name, body_pose);
251 }
252 
259 void vpMocapVicon::setServerAddress(const std::string &serverAddr) { m_impl->setServerAddress(serverAddr); }
260 
265 void 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
270 void 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)