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();
This file is responsible for the initialisation of the world parameters that are controlled by ODE.
void setSimulationIMU(iCubSimulationIMU *imu)
void removeSimulationIMU()
bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
size_t getNrOfThreeAxisLinearAccelerometers() const override
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
size_t getNrOfOrientationSensors() const override
virtual ~iCubSimulationIMU()
bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
size_t getNrOfThreeAxisGyroscopes() const override
size_t getNrOfThreeAxisMagnetometers() const override
bool open(yarp::os::Searchable &config) override
bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
void updateIMUData(const yarp::os::Bottle &imuData)
bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
bool getOrientationSensorName(size_t sens_index, std::string &name) const override
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double ×tamp) const override
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double ×tamp) const override
bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override