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;
51 #ifndef DOXYGEN_SHOULD_SKIP_THIS
52 class vpMocapVicon::vpMocapViconImpl
55 vpMocapViconImpl() : m_DirectClient(), m_verbose(false), m_serverAddr() {}
56 virtual ~vpMocapViconImpl() { close(); }
60 m_DirectClient.DisableSegmentData();
61 m_DirectClient.DisableMarkerData();
62 m_DirectClient.DisableUnlabeledMarkerData();
63 m_DirectClient.DisableDeviceData();
65 std::cout <<
"Disconnecting..." << std::endl;
67 m_DirectClient.Disconnect();
73 for (
auto i = 0; i < n_attempt; i++) {
74 if (!m_DirectClient.IsConnected().Connected) {
77 const Output_Connect ConnectResult = m_DirectClient.Connect(m_serverAddr);
78 const bool ok = (ConnectResult.Result == Result::Success);
82 std::cout <<
"Warning - connection failed... ";
83 switch (ConnectResult.Result) {
84 case Result::ClientAlreadyConnected:
85 std::cout <<
"Client Already Connected" << std::endl;
87 case Result::InvalidHostName:
88 std::cout <<
"Invalid Host Name" << std::endl;
90 case Result::ClientConnectionFailed:
91 std::cout <<
"Client Connection Failed" << std::endl;
94 std::cout <<
"Unrecognized Error: " << ConnectResult.Result << std::endl;
102 std::cout <<
"Successful connection to : " << m_serverAddr << std::endl;
104 return setupDataStreamed();
110 std::cout <<
"Vicon connection timeout" << std::endl;
115 bool getBodiesPose(std::map<std::string, vpHomogeneousMatrix> &bodies_pose,
bool all_bodies =
false)
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);
125 if (!data_extraction_success) {
126 std::cout <<
"Error : Could not get pose from body n°" << iBody << std::endl;
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;
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++];
144 bodies_pose[bodyName] = bodyPose;
145 }
else if (bodyPose.
isValid()) {
146 bodies_pose[bodyName] = bodyPose;
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];
162 std::cout <<
"The body " << body_name <<
" was not found in Vicon. Please check the name you typed."
168 std::cout <<
"Error : could not process data from Vicon" << std::endl;
174 void setServerAddress(
const std::string &serverAddr) { m_serverAddr = serverAddr; }
176 void setVerbose(
bool verbose) { m_verbose = verbose; }
178 bool setupDataStreamed()
181 m_DirectClient.EnableSegmentData();
182 m_DirectClient.EnableMarkerData();
183 m_DirectClient.EnableUnlabeledMarkerData();
184 m_DirectClient.EnableMarkerRayData();
185 m_DirectClient.EnableDeviceData();
186 m_DirectClient.EnableDebugData();
188 m_DirectClient.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush);
191 m_DirectClient.SetAxisMapping(Direction::Forward, Direction::Left,
198 ViconDataStreamSDK::CPP::Client m_DirectClient;
200 std::string m_serverAddr;
239 return m_impl->getBodiesPose(bodies_pose, all_bodies);
250 return m_impl->getSpecificBodyPose(body_name, body_pose);
270 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)