10 #include <sot/core/debug.hh> 11 #include <dynamic-graph/factory.h> 13 #include <dynamic-graph/all-commands.h> 14 #include <sot/core/stop-watch.hh> 26 namespace talos_balance
33 #define INPUT_SIGNALS m_dummySIN 34 #define OUTPUT_SIGNALS 50 ,CONSTRUCT_SIGNAL_IN(dummy, double)
51 ,m_initSucceeded(false)
57 docCommandVoid0(
"Initialize the entity.")));
59 addCommand(
"registerRigidBody",
61 docCommandVoid1(
"Register a rigid body",
62 "Name of the rigid body")));
63 addCommand(
"setMocapIPAdress",
65 docCommandVoid1(
"Set IP adress of the Mocap server",
66 "IP adress string")));
69 addCommand(
"getRigidBodyList",
71 docCommandVoid0(
"Displays the list of rigid bodies streamed by the mocap server")));
103 Vector7d initRBposition;
104 initRBposition << RBidx,0,0,0,0,0,0;
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);
109 signalRegistration( *sig );
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;
151 if (!rtProtocol.
Connect(
m_serverAddr.c_str(), basePort, &udpPort, majorVersion, minorVersion, bigEndian))
153 printf(
"rtProtocol.Connect: %s\n\n", rtProtocol.
GetErrorString());
163 printf(
"rtProtocol.Read6DOFSettings: %s\n\n", rtProtocol.
GetErrorString());
173 printf(
"rtProtocol.StreamFrames: %s\n\n", rtProtocol.
GetErrorString());
185 float rotationMatrix[9];
192 if (rtPacket->
Get6DOFBody(i, fX, fY, fZ, rotationMatrix))
194 Eigen::Matrix3f rotMat = Eigen::Map<Eigen::Matrix3f>(rotationMatrix);
195 Eigen::Quaternionf quat(rotMat);
199 SEND_MSG(
" - "+toString(pTmpStr), MSG_TYPE_INFO);
200 std::cout << (
" - "+toString(pTmpStr)+
"\n");
206 std::vector<std::string>::iterator it = std::find(
m_RBnames.begin(),
m_RBnames.end(), pTmpStr);
210 m_RBpositions[idx] << fX, fY, fZ, quat.w(), quat.x(), quat.y(), quat.z();
233 os <<
"QualisysClient "<<getName();
236 getProfiler().report_all(3, os);
238 catch (ExceptionSignal e) {}
const char * Get6DOFBodyName(unsigned int nBodyIndex) const
void setMocapIPAdress(const std::string &ipAdress)
void registerRigidBody(const std::string &RBname)
void manageNetworkFrame()
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
static const unsigned int cComponent6d
AdmittanceControllerEndEffector EntityClassName
bool m_printRigidBodyList
std::vector< std::string > m_RBnames
bool Get6DOFBody(unsigned int nBodyIndex, float &fX, float &fY, float &fZ, float afRotMatrix[9])
EIGEN_MAKE_ALIGNED_OPERATOR_NEW QualisysClient(const std::string &name)
std::vector< dg::Vector > m_RBpositions
bool Read6DOFSettings(bool &bDataAvailable)
unsigned int Get6DOFBodyCount()
bool Connect(const char *pServerAddr, unsigned short nPort, unsigned short *pnUDPServerPort=nullptr, int nMajorVersion=MAJOR_VERSION, int nMinorVersion=MINOR_VERSION, bool bBigEndian=false)
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={ })
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(QualisysClient, "QualisysClient")
int ReceiveRTPacket(CRTPacket::EPacketType &eType, bool bSkipEvents=true, int nTimeout=WAIT_FOR_DATA_TIMEOUT)