36 #include <visp3/core/vpConfig.h>
38 #ifdef VISP_HAVE_VICON
43 #include <visp3/core/vpTime.h>
44 #include <visp3/sensor/vpMocapVicon.h>
46 #include <DataStreamClient.h>
47 #include <IDataStreamClientBase.h>
49 using namespace ViconDataStreamSDK::CPP;
52 #ifndef DOXYGEN_SHOULD_SKIP_THIS
53 class vpMocapVicon::vpMocapViconImpl
56 vpMocapViconImpl() : m_DirectClient(), m_verbose(false), m_serverAddr() { }
57 virtual ~vpMocapViconImpl() { close(); }
61 m_DirectClient.DisableSegmentData();
62 m_DirectClient.DisableMarkerData();
63 m_DirectClient.DisableUnlabeledMarkerData();
64 m_DirectClient.DisableDeviceData();
66 std::cout <<
"Disconnecting..." << std::endl;
68 m_DirectClient.Disconnect();
74 for (
auto i = 0; i < n_attempt; i++) {
75 if (!m_DirectClient.IsConnected().Connected) {
78 const Output_Connect ConnectResult = m_DirectClient.Connect(m_serverAddr);
79 const bool ok = (ConnectResult.Result == Result::Success);
83 std::cout <<
"Warning - connection failed... ";
84 switch (ConnectResult.Result) {
85 case Result::ClientAlreadyConnected:
86 std::cout <<
"Client Already Connected" << std::endl;
88 case Result::InvalidHostName:
89 std::cout <<
"Invalid Host Name" << std::endl;
91 case Result::ClientConnectionFailed:
92 std::cout <<
"Client Connection Failed" << std::endl;
95 std::cout <<
"Unrecognized Error: " << ConnectResult.Result << std::endl;
103 std::cout <<
"Successful connection to : " << m_serverAddr << std::endl;
105 return setupDataStreamed();
111 std::cout <<
"Vicon connection timeout" << std::endl;
116 bool getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose,
bool all_bodies =
false)
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);
126 if (!data_extraction_success) {
127 std::cout <<
"Error : Could not get pose from body n°" << iBody << std::endl;
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;
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++];
146 bodies_pose[bodyName] = bodyPose;
149 bodies_pose[bodyName] = bodyPose;
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];
166 std::cout <<
"The body " << body_name <<
" was not found in Vicon. Please check the name you typed."
173 std::cout <<
"Error : could not process data from Vicon" << std::endl;
179 void setServerAddress(
const std::string &serverAddr) { m_serverAddr = serverAddr; }
181 void setVerbose(
bool verbose) { m_verbose = verbose; }
183 bool setupDataStreamed()
186 m_DirectClient.EnableSegmentData();
187 m_DirectClient.EnableMarkerData();
188 m_DirectClient.EnableUnlabeledMarkerData();
189 m_DirectClient.EnableMarkerRayData();
190 m_DirectClient.EnableDeviceData();
191 m_DirectClient.EnableDebugData();
193 m_DirectClient.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
196 m_DirectClient.SetAxisMapping(Direction::Forward, Direction::Left,
203 ViconDataStreamSDK::CPP::Client m_DirectClient;
205 std::string m_serverAddr;
244 return m_impl->getBodiesPose(bodies_pose, all_bodies);
255 return m_impl->getSpecificBodyPose(body_name, body_pose);
275 void dummy_vpMocapVicon() { }
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)
void setServerAddress(const std::string &serverAddr)
bool getSpecificBodyPose(const std::string &body_name, vpHomogeneousMatrix &body_pose)
VISP_EXPORT void sleepMs(double t)