10 #include <yarp/os/Stamp.h>
11 #include <yarp/os/LogStream.h>
17 using namespace yarp::os;
19 using namespace yarp::sig;
26 accels.resize(3, 0.0);
28 m_sensorName =
"sensorName";
29 m_frameName =
"frameName";
35 lock_guard<mutex> lck(odeinit.
mtx);
42 lock_guard<mutex> lck(odeinit.
mtx);
55 yarp::dev::MAS_status iCubSimulationIMU::genericGetStatus(
size_t sens_index)
const
58 return yarp::dev::MAS_status::MAS_ERROR;
61 return yarp::dev::MAS_status::MAS_OK;
64 bool iCubSimulationIMU::genericGetSensorName(
size_t sens_index,
std::string &name)
const
74 bool iCubSimulationIMU::genericGetFrameName(
size_t sens_index,
std::string &frameName)
const
80 frameName = m_frameName;
91 return genericGetStatus(sens_index);
96 return genericGetSensorName(sens_index, name);
101 return genericGetFrameName(sens_index, frameName);
116 yarp::os::Stamp copyStamp(lastStamp);
117 timestamp = copyStamp.getTime();
129 return genericGetStatus(sens_index);
134 return genericGetSensorName(sens_index, name);
139 return genericGetFrameName(sens_index, frameName);
154 yarp::os::Stamp copyStamp(lastStamp);
155 timestamp = copyStamp.getTime();
167 return genericGetStatus(sens_index);
172 return genericGetSensorName(sens_index, name);
177 return genericGetFrameName(sens_index, frameName);
192 yarp::os::Stamp copyStamp(lastStamp);
193 timestamp = copyStamp.getTime();
205 return genericGetStatus(sens_index);
210 return genericGetSensorName(sens_index, name);
215 return genericGetFrameName(sens_index, frameName);
230 yarp::os::Stamp copyStamp(lastStamp);
231 timestamp = copyStamp.getTime();
239 if (imuData.size() < 12) {
242 rpy[0] = imuData.get(0).asFloat64();
243 rpy[1] = imuData.get(1).asFloat64();
244 rpy[2] = imuData.get(2).asFloat64();
246 accels[0] = imuData.get(3).asFloat64();
247 accels[1] = imuData.get(4).asFloat64();
248 accels[2] = imuData.get(5).asFloat64();
250 gyro[0] = imuData.get(6).asFloat64();
251 gyro[1] = imuData.get(7).asFloat64();
252 gyro[2] = imuData.get(8).asFloat64();
254 magn[0] = imuData.get(9).asFloat64();
255 magn[1] = imuData.get(10).asFloat64();
256 magn[2] = imuData.get(11).asFloat64();