Visual Servoing Platform  version 3.6.1 under development (2024-12-09)
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 
49 using namespace ViconDataStreamSDK::CPP;
50 
51 BEGIN_VISP_NAMESPACE
52 #ifndef DOXYGEN_SHOULD_SKIP_THIS
53 class vpMocapVicon::vpMocapViconImpl
54 {
55 public:
56  vpMocapViconImpl() : m_DirectClient(), m_verbose(false), m_serverAddr() { }
57  virtual ~vpMocapViconImpl() { close(); }
58 
59  void close()
60  {
61  m_DirectClient.DisableSegmentData();
62  m_DirectClient.DisableMarkerData();
63  m_DirectClient.DisableUnlabeledMarkerData();
64  m_DirectClient.DisableDeviceData();
65  if (m_verbose) {
66  std::cout << "Disconnecting..." << std::endl;
67  }
68  m_DirectClient.Disconnect();
69  }
70 
71  bool connect()
72  {
73  int n_attempt = 2;
74  for (auto i = 0; i < n_attempt; i++) {
75  if (!m_DirectClient.IsConnected().Connected) {
76  // Direct connection
77 
78  const Output_Connect ConnectResult = m_DirectClient.Connect(m_serverAddr);
79  const bool ok = (ConnectResult.Result == Result::Success);
80 
81  if (!ok) {
82  if (m_verbose) {
83  std::cout << "Warning - connection failed... ";
84  switch (ConnectResult.Result) {
85  case Result::ClientAlreadyConnected:
86  std::cout << "Client Already Connected" << std::endl;
87  break;
88  case Result::InvalidHostName:
89  std::cout << "Invalid Host Name" << std::endl;
90  break;
91  case Result::ClientConnectionFailed:
92  std::cout << "Client Connection Failed" << std::endl;
93  break;
94  default:
95  std::cout << "Unrecognized Error: " << ConnectResult.Result << std::endl;
96  break;
97  }
98  }
99  vpTime::sleepMs(1000);
100  }
101  if (ok) {
102  if (m_verbose) {
103  std::cout << "Successful connection to : " << m_serverAddr << std::endl;
104  }
105  return setupDataStreamed();
106  }
107  }
108  }
109 
110  if (m_verbose) {
111  std::cout << "Vicon connection timeout" << std::endl;
112  }
113  return false;
114  }
115 
116  bool getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies = false)
117  {
118  if (m_DirectClient.GetFrame().Result == Result::Success) {
119  for (unsigned int iBody = 0; iBody < m_DirectClient.GetSubjectCount().SubjectCount; iBody++) {
120  std::string bodyName = m_DirectClient.GetSubjectName(iBody).SubjectName;
121  std::string rootSegment = m_DirectClient.GetSubjectRootSegmentName(bodyName).SegmentName;
122  bool data_extraction_success = (m_DirectClient.GetSegmentGlobalRotationMatrix(bodyName, rootSegment).Result &&
123  m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Result);
124  vpHomogeneousMatrix bodyPose;
125 
126  if (!data_extraction_success) {
127  std::cout << "Error : Could not get pose from body n°" << iBody << std::endl;
128 
129  return false;
130  }
131  else {
132  bodyPose[0][3] = m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Translation[0] / 1000.0;
133  bodyPose[1][3] = m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Translation[1] / 1000.0;
134  bodyPose[2][3] = m_DirectClient.GetSegmentGlobalTranslation(bodyName, rootSegment).Translation[2] / 1000.0;
135  bodyPose[3][3] = 1.0;
136 
137  // Vicon is row major
138  unsigned int k = 0;
139  for (unsigned int i = 0; i < 3; i++) {
140  for (unsigned int j = 0; j < 3; j++) {
141  bodyPose[i][j] = m_DirectClient.GetSegmentGlobalRotationMatrix(bodyName, rootSegment).Rotation[k++];
142  }
143  }
144  }
145  if (all_bodies) {
146  bodies_pose[bodyName] = bodyPose;
147  }
148  else if (bodyPose.isValid()) {
149  bodies_pose[bodyName] = bodyPose;
150  }
151  }
152  return true;
153  }
154  return false;
155  }
156 
157  bool getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
158  {
159  std::map<std::string, vpHomogeneousMatrix> bodies_pose;
160  if (getBodiesPose(bodies_pose, true)) {
161  if (bodies_pose.find(body_name) != bodies_pose.end()) {
162  body_pose = bodies_pose[body_name];
163  return true;
164  }
165  else {
166  std::cout << "The body " << body_name << " was not found in Vicon. Please check the name you typed."
167  << std::endl;
168 
169  return false;
170  }
171  }
172  else {
173  std::cout << "Error : could not process data from Vicon" << std::endl;
174 
175  return false;
176  }
177  }
178 
179  void setServerAddress(const std::string &serverAddr) { m_serverAddr = serverAddr; }
180 
181  void setVerbose(bool verbose) { m_verbose = verbose; }
182 
183  bool setupDataStreamed()
184  {
185  // We set up the kind of data we get.
186  m_DirectClient.EnableSegmentData();
187  m_DirectClient.EnableMarkerData();
188  m_DirectClient.EnableUnlabeledMarkerData();
189  m_DirectClient.EnableMarkerRayData();
190  m_DirectClient.EnableDeviceData();
191  m_DirectClient.EnableDebugData();
192  // We set up which kind of connectiion we want.
193  m_DirectClient.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
194 
195  // Set the global up axis
196  m_DirectClient.SetAxisMapping(Direction::Forward, Direction::Left,
197  Direction::Up); // Z-up
198 
199  return true;
200  }
201 
202 private:
203  ViconDataStreamSDK::CPP::Client m_DirectClient;
204  bool m_verbose;
205  std::string m_serverAddr;
206 };
207 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
208 
209 /*
210  **********************************************************************************************
211  */
212 
216 vpMocapVicon::vpMocapVicon() : m_impl(new vpMocapViconImpl()) { }
217 
221 vpMocapVicon::~vpMocapVicon() { delete m_impl; }
222 
226 void vpMocapVicon::close() { m_impl->close(); }
227 
233 bool vpMocapVicon::connect() { return m_impl->connect(); }
234 
242 bool vpMocapVicon::getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose, bool all_bodies)
243 {
244  return m_impl->getBodiesPose(bodies_pose, all_bodies);
245 }
246 
253 bool vpMocapVicon::getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
254 {
255  return m_impl->getSpecificBodyPose(body_name, body_pose);
256 }
257 
264 void vpMocapVicon::setServerAddress(const std::string &serverAddr) { m_impl->setServerAddress(serverAddr); }
265 
270 void vpMocapVicon::setVerbose(bool verbose) { m_impl->setVerbose(verbose); }
271 END_VISP_NAMESPACE
272 #else
273 // Work around to avoid warning:
274 // libvisp_sensor.a(vpMocapVicon.cpp.o) has no symbols
275 void dummy_vpMocapVicon() { }
276 #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)