sot-talos-balance  1.6.0
qualisys-client.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2019
3  * LAAS-CNRS
4  * F. Bailly
5  * T. Flayols
6  */
7 
8 
10 #include <sot/core/debug.hh>
11 #include <dynamic-graph/factory.h>
12 
13 #include <dynamic-graph/all-commands.h>
14 #include <sot/core/stop-watch.hh>
16 
17 // #include "RTProtocol.h"
18 // #include "RTPacket.h"
19 
20 using namespace sot::talos_balance;
21 
22 namespace dynamicgraph
23 {
24  namespace sot
25  {
26  namespace talos_balance
27  {
28  namespace dynamicgraph = ::dynamicgraph;
29  using namespace dynamicgraph;
30  using namespace dynamicgraph::command;
31  using namespace dg::sot::talos_balance;
32 
33 #define INPUT_SIGNALS m_dummySIN
34 #define OUTPUT_SIGNALS
35 
38  typedef QualisysClient EntityClassName;
39 
40  /* --- DG FACTORY ---------------------------------------------------- */
42  "QualisysClient");
43 
44  /* ------------------------------------------------------------------- */
45  /* --- CONSTRUCTION -------------------------------------------------- */
46  /* ------------------------------------------------------------------- */
48  QualisysClient(const std::string& name)
49  : Entity(name)
50  ,CONSTRUCT_SIGNAL_IN(dummy, double)
51  ,m_initSucceeded(false)
52  {
53  Entity::signalRegistration( INPUT_SIGNALS );
54  /* Commands. */
55  addCommand("init",
56  makeCommandVoid0(*this, &QualisysClient::init,
57  docCommandVoid0("Initialize the entity.")));
58 
59  addCommand("registerRigidBody",
60  makeCommandVoid1(*this,&QualisysClient::registerRigidBody,
61  docCommandVoid1("Register a rigid body",
62  "Name of the rigid body")));
63  addCommand("setMocapIPAdress",
64  makeCommandVoid1(*this,&QualisysClient::setMocapIPAdress,
65  docCommandVoid1("Set IP adress of the Mocap server",
66  "IP adress string")));
67 
68 
69  addCommand("getRigidBodyList",
70  makeCommandVoid0(*this,&QualisysClient::getRigidBodyList,
71  docCommandVoid0("Displays the list of rigid bodies streamed by the mocap server")));
72 
73  }
74 
76  {
77  m_initSucceeded = true;
78  m_dummySIN = 0;
79  // m_thread.join();
80  // m_CRTProtocol rtProtocol;
81  // const char m_serverAddr[] = "140.93.6.44";
82  // const unsigned short m_basePort = 22222;
83  // const int m_majorVersion = 1;
84  // const int m_minorVersion = 19;
85  // const bool m_bigEndian = false;
86  // bool m_dataAvailable = false;
87  // bool m_streamFrames = false;
88  // unsigned short m_udpPort = 6734;
89  }
90 
91 
92  /* ------------------------------------------------------------------- */
93  /* --- SIGNALS ------------------------------------------------------- */
94  /* ------------------------------------------------------------------- */
95 
96 
97  /* --- COMMANDS ---------------------------------------------------------- */
98 
99  void QualisysClient::registerRigidBody(const std::string& RBname)
100  {
101  m_RBnames.push_back(RBname);
102  int RBidx = m_RBnames.size()-1;
103  Vector7d initRBposition;
104  initRBposition << RBidx,0,0,0,0,0,0;
105  m_RBpositions.push_back(initRBposition);
106  dg::SignalTimeDependent< dg::Vector,int > * sig;
107  sig = new dg::SignalTimeDependent< dg::Vector,int >(boost::bind(&QualisysClient::readGenericRigidBody,this,RBidx,_1,_2),m_dummySIN,getClassName()+"("+getName()+")::output(dynamicgraph::Vector)::xyzquat_"+RBname);
108  //genericSignalRefs.push_back( sig );
109  signalRegistration( *sig );
110  }
111 
112  void QualisysClient::setMocapIPAdress(const std::string& ipAdress)
113  {
114  m_serverAddr = ipAdress;
115  }
116 
118  {
119  m_printRigidBodyList = true;
120  }
121 
122  /* --- PROTECTED MEMBER METHODS ---------------------------------------------------------- */
123 
124  dg::Vector& QualisysClient::readGenericRigidBody(const int RBidx, dg::Vector& res, const int& time)
125  {
126  if(res.size()!=7)
127  {
128  res.resize(7);
129  }
130  m_mutex.lock();
131  res << m_RBpositions[RBidx];
132  m_mutex.unlock();
133  return res;
134  }
135 
137  {
138  CRTProtocol rtProtocol;
139  const unsigned short basePort = 22222;
140  const int majorVersion = 1;
141  const int minorVersion = 19;
142  const bool bigEndian = false;
143  bool dataAvailable = false;
144  bool streamFrames = false;
145  unsigned short udpPort = 6734;
146 
147  while (true)
148  {
149  if (!rtProtocol.Connected())
150  {
151  if (!rtProtocol.Connect(m_serverAddr.c_str(), basePort, &udpPort, majorVersion, minorVersion, bigEndian))
152  {
153  printf("rtProtocol.Connect: %s\n\n", rtProtocol.GetErrorString());
154  sleep(1);
155  continue;
156  }
157  }
158 
159  if (!dataAvailable)
160  {
161  if (!rtProtocol.Read6DOFSettings(dataAvailable))
162  {
163  printf("rtProtocol.Read6DOFSettings: %s\n\n", rtProtocol.GetErrorString());
164  sleep(1);
165  continue;
166  }
167  }
168 
169  if (!streamFrames)
170  {
171  if (!rtProtocol.StreamFrames(CRTProtocol::RateAllFrames, 0, udpPort, NULL, CRTProtocol::cComponent6d))
172  {
173  printf("rtProtocol.StreamFrames: %s\n\n", rtProtocol.GetErrorString());
174  sleep(1);
175  continue;
176  }
177  }
178  CRTPacket::EPacketType packetType;
179 
180  if (rtProtocol.ReceiveRTPacket(packetType, true) > 0)
181  {
182  if (packetType == CRTPacket::PacketData)
183  {
184  float fX, fY, fZ;
185  float rotationMatrix[9];
186 
187  CRTPacket* rtPacket = rtProtocol.GetRTPacket();
188 
189  //printf("Frame %d\n", rtPacket->GetFrameNumber());
190  for (unsigned int i = 0; i < rtPacket->Get6DOFBodyCount(); i++)
191  {
192  if (rtPacket->Get6DOFBody(i, fX, fY, fZ, rotationMatrix))
193  {
194  Eigen::Matrix3f rotMat = Eigen::Map<Eigen::Matrix3f>(rotationMatrix);
195  Eigen::Quaternionf quat(rotMat);
196  const char* pTmpStr = rtProtocol.Get6DOFBodyName(i);
198  {
199  SEND_MSG(" - "+toString(pTmpStr), MSG_TYPE_INFO);
200  std::cout << (" - "+toString(pTmpStr)+"\n"); //Todo remove
201  }
202  if (pTmpStr)
203  {
204  //compare pTmpStr with m_RBnames
205  m_mutex.lock();
206  std::vector<std::string>::iterator it = std::find(m_RBnames.begin(), m_RBnames.end(), pTmpStr);
207  if (it != m_RBnames.end())
208  {
209  int idx = it - m_RBnames.begin();
210  m_RBpositions[idx] << fX, fY, fZ, quat.w(), quat.x(), quat.y(), quat.z();
211  }
212  m_mutex.unlock();
213  }
214  }
215  }
216  m_printRigidBodyList = false;
217  }
218  }
219 
220 
221  // boost::this_thread::sleep_for(boost::chrono::seconds{1});
222  }
223  rtProtocol.StopCapture();
224  rtProtocol.Disconnect();
225  }
226  /* ------------------------------------------------------------------- */
227  /* --- ENTITY -------------------------------------------------------- */
228  /* ------------------------------------------------------------------- */
229 
230 
231  void QualisysClient::display(std::ostream& os) const
232  {
233  os << "QualisysClient "<<getName();
234  try
235  {
236  getProfiler().report_all(3, os);
237  }
238  catch (ExceptionSignal e) {}
239  }
240 
241  } // namespace talos_balance
242  } // namespace sot
243 } // namespace dynamicgraph
void Disconnect()
Definition: RTProtocol.cpp:212
const char * Get6DOFBodyName(unsigned int nBodyIndex) const
void setMocapIPAdress(const std::string &ipAdress)
void registerRigidBody(const std::string &RBname)
virtual void display(std::ostream &os) const
If connected, return the list of all the rigid bodies available.
CRTPacket * GetRTPacket()
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: math/fwd.hh:40
static const unsigned int cComponent6d
Definition: RTProtocol.h:35
bool StopCapture()
Definition: RTProtocol.cpp:854
bool Get6DOFBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, float afRotMatrix[9])
Definition: RTPacket.cpp:816
EIGEN_MAKE_ALIGNED_OPERATOR_NEW QualisysClient(const std::string &name)
bool Read6DOFSettings(bool &bDataAvailable)
#define INPUT_SIGNALS
unsigned int Get6DOFBodyCount()
Definition: RTPacket.cpp:805
bool Connect(const char *pServerAddr, unsigned short nPort, unsigned short *pnUDPServerPort=nullptr, int nMajorVersion=MAJOR_VERSION, int nMinorVersion=MINOR_VERSION, bool bBigEndian=false)
Definition: RTProtocol.cpp:82
bool Connected() const
Definition: RTProtocol.cpp:225
char * GetErrorString()
dg::Vector & readGenericRigidBody(const int RBidx, dg::Vector &res, const int &time)
bool StreamFrames(EStreamRate eRate, unsigned int nRateArg, unsigned short nUDPPort, const char *pUDPAddr, unsigned int nComponentType, const SComponentOptions &componentOptions={ })
Definition: RTProtocol.cpp:434
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(QualisysClient, "QualisysClient")
int ReceiveRTPacket(CRTPacket::EPacketType &eType, bool bSkipEvents=true, int nTimeout=WAIT_FOR_DATA_TIMEOUT)