iCub-main
iCubSimulationIMU.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #include <string>
10 #include <yarp/os/Stamp.h>
11 #include <yarp/os/LogStream.h>
12 
13 #include <iCubSimulationIMU.h>
14 
15 #include "OdeInit.h"
16 
17 using namespace yarp::os;
18 using namespace yarp::dev;
19 using namespace yarp::sig;
20 
21 
23 {
24  gyro.resize(3,0.0);
25  rpy.resize(3,0.0);
26  accels.resize(3, 0.0);
27  magn.resize(3,0.0);
28  m_sensorName = "sensorName";
29  m_frameName = "frameName";
30 }
31 
33 {
34  OdeInit& odeinit = OdeInit::get();
35  lock_guard<mutex> lck(odeinit.mtx);
36  odeinit.removeSimulationIMU();
37 }
38 
39 bool iCubSimulationIMU::open(yarp::os::Searchable &config)
40 {
41  OdeInit& odeinit = OdeInit::get();
42  lock_guard<mutex> lck(odeinit.mtx);
43  odeinit.setSimulationIMU(this);
44  return true;
45 }
46 
48 {
49  // TODO see what does simulation control
50  return true;
51 }
52 
53 
54 
55 yarp::dev::MAS_status iCubSimulationIMU::genericGetStatus(size_t sens_index) const
56 {
57  if (sens_index!=0) {
58  return yarp::dev::MAS_status::MAS_ERROR;
59  }
60 
61  return yarp::dev::MAS_status::MAS_OK;
62 }
63 
64 bool iCubSimulationIMU::genericGetSensorName(size_t sens_index, std::string &name) const
65 {
66  if (sens_index!=0) {
67  return false;
68  }
69 
70  name = m_sensorName;
71  return true;
72 }
73 
74 bool iCubSimulationIMU::genericGetFrameName(size_t sens_index, std::string &frameName) const
75 {
76  if (sens_index!=0) {
77  return false;
78  }
79 
80  frameName = m_frameName;
81  return true;
82 }
83 
85 {
86  return 1;
87 }
88 
89 yarp::dev::MAS_status iCubSimulationIMU::getThreeAxisGyroscopeStatus(size_t sens_index) const
90 {
91  return genericGetStatus(sens_index);
92 }
93 
94 bool iCubSimulationIMU::getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const
95 {
96  return genericGetSensorName(sens_index, name);
97 }
98 
99 bool iCubSimulationIMU::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const
100 {
101  return genericGetFrameName(sens_index, frameName);
102 }
103 
104 bool iCubSimulationIMU::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
105 {
106  if (sens_index!=0) {
107  return false;
108  }
109 
110  out.resize(3);
111  m_mutex.lock();
112  out = gyro;
113  m_mutex.unlock();
114 
115  // Workaround for https://github.com/robotology/yarp/issues/1610
116  yarp::os::Stamp copyStamp(lastStamp);
117  timestamp = copyStamp.getTime();
118 
119  return true;
120 }
121 
123 {
124  return 1;
125 }
126 
127 yarp::dev::MAS_status iCubSimulationIMU::getThreeAxisLinearAccelerometerStatus(size_t sens_index) const
128 {
129  return genericGetStatus(sens_index);
130 }
131 
133 {
134  return genericGetSensorName(sens_index, name);
135 }
136 
138 {
139  return genericGetFrameName(sens_index, frameName);
140 }
141 
142 bool iCubSimulationIMU::getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
143 {
144  if (sens_index!=0) {
145  return false;
146  }
147 
148  out.resize(3);
149  m_mutex.lock();
150  out = accels;
151  m_mutex.unlock();
152 
153  // Workaround for https://github.com/robotology/yarp/issues/1610
154  yarp::os::Stamp copyStamp(lastStamp);
155  timestamp = copyStamp.getTime();
156 
157  return true;
158 }
159 
161 {
162  return 1;
163 }
164 
165 yarp::dev::MAS_status iCubSimulationIMU::getThreeAxisMagnetometerStatus(size_t sens_index) const
166 {
167  return genericGetStatus(sens_index);
168 }
169 
171 {
172  return genericGetSensorName(sens_index, name);
173 }
174 
175 bool iCubSimulationIMU::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const
176 {
177  return genericGetFrameName(sens_index, frameName);
178 }
179 
180 bool iCubSimulationIMU::getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
181 {
182  if (sens_index!=0) {
183  return false;
184  }
185 
186  out.resize(3);
187  m_mutex.lock();
188  out = magn;
189  m_mutex.unlock();
190 
191  // Workaround for https://github.com/robotology/yarp/issues/1610
192  yarp::os::Stamp copyStamp(lastStamp);
193  timestamp = copyStamp.getTime();
194 
195  return true;
196 }
197 
199 {
200  return 1;
201 }
202 
203 yarp::dev::MAS_status iCubSimulationIMU::getOrientationSensorStatus(size_t sens_index) const
204 {
205  return genericGetStatus(sens_index);
206 }
207 
208 bool iCubSimulationIMU::getOrientationSensorName(size_t sens_index, std::string &name) const
209 {
210  return genericGetSensorName(sens_index, name);
211 }
212 
213 bool iCubSimulationIMU::getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const
214 {
215  return genericGetFrameName(sens_index, frameName);
216 }
217 
218 bool iCubSimulationIMU::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy_out, double& timestamp) const
219 {
220  if (sens_index!=0) {
221  return false;
222  }
223 
224  rpy_out.resize(3);
225  m_mutex.lock();
226  rpy_out = rpy;
227  m_mutex.unlock();
228 
229  // Workaround for https://github.com/robotology/yarp/issues/1610
230  yarp::os::Stamp copyStamp(lastStamp);
231  timestamp = copyStamp.getTime();
232 
233  return true;
234 }
235 
236 
237 void iCubSimulationIMU::updateIMUData(const yarp::os::Bottle& imuData) {
238  m_mutex.lock();
239  if (imuData.size() < 12) {
240  return;
241  }
242  rpy[0] = imuData.get(0).asFloat64();
243  rpy[1] = imuData.get(1).asFloat64();
244  rpy[2] = imuData.get(2).asFloat64();
245 
246  accels[0] = imuData.get(3).asFloat64();
247  accels[1] = imuData.get(4).asFloat64();
248  accels[2] = imuData.get(5).asFloat64();
249 
250  gyro[0] = imuData.get(6).asFloat64();
251  gyro[1] = imuData.get(7).asFloat64();
252  gyro[2] = imuData.get(8).asFloat64();
253 
254  magn[0] = imuData.get(9).asFloat64();
255  magn[1] = imuData.get(10).asFloat64();
256  magn[2] = imuData.get(11).asFloat64();
257 
258  m_mutex.unlock();
259 }
iCubSimulationIMU::getNrOfThreeAxisLinearAccelerometers
size_t getNrOfThreeAxisLinearAccelerometers() const override
Definition: iCubSimulationIMU.cpp:122
iCubSimulationIMU::updateIMUData
void updateIMUData(const yarp::os::Bottle &imuData)
Definition: iCubSimulationIMU.cpp:237
iCubSimulationIMU::open
bool open(yarp::os::Searchable &config) override
Definition: iCubSimulationIMU.cpp:39
iCubSimulationIMU::close
bool close() override
Definition: iCubSimulationIMU.cpp:47
iCubSimulationIMU::getThreeAxisGyroscopeMeasure
bool getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Definition: iCubSimulationIMU.cpp:104
out
out
Definition: sine.m:8
iCubSimulationIMU::getThreeAxisMagnetometerFrameName
bool getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
Definition: iCubSimulationIMU.cpp:175
iCubSimulationIMU::getThreeAxisMagnetometerMeasure
bool getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Definition: iCubSimulationIMU.cpp:180
iCubSimulationIMU::getThreeAxisLinearAccelerometerMeasure
bool getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Definition: iCubSimulationIMU.cpp:142
iCubSimulationIMU.h
yarp::dev
Definition: DebugInterfaces.h:52
iCubSimulationIMU::getOrientationSensorStatus
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Definition: iCubSimulationIMU.cpp:203
OdeInit.h
This file is responsible for the initialisation of the world parameters that are controlled by ODE....
iCubSimulationIMU::getThreeAxisGyroscopeFrameName
bool getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Definition: iCubSimulationIMU.cpp:99
OdeInit::setSimulationIMU
void setSimulationIMU(iCubSimulationIMU *imu)
Definition: OdeInit.cpp:133
iCubSimulationIMU::getThreeAxisMagnetometerName
bool getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
Definition: iCubSimulationIMU.cpp:170
iCubSimulationIMU::getThreeAxisLinearAccelerometerFrameName
bool getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Definition: iCubSimulationIMU.cpp:137
iCubSimulationIMU::getOrientationSensorMeasureAsRollPitchYaw
bool getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
Definition: iCubSimulationIMU.cpp:218
iCubSimulationIMU::getThreeAxisLinearAccelerometerStatus
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Definition: iCubSimulationIMU.cpp:127
OdeInit::get
static OdeInit & get()
Definition: OdeInit.cpp:189
iCubSimulationIMU::getThreeAxisGyroscopeStatus
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Definition: iCubSimulationIMU.cpp:89
iCubSimulationIMU::iCubSimulationIMU
iCubSimulationIMU()
Definition: iCubSimulationIMU.cpp:22
iCubSimulationIMU::getNrOfOrientationSensors
size_t getNrOfOrientationSensors() const override
Definition: iCubSimulationIMU.cpp:198
iCubSimulationIMU::getThreeAxisMagnetometerStatus
yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
Definition: iCubSimulationIMU.cpp:165
iCubSimulationIMU::getThreeAxisGyroscopeName
bool getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Definition: iCubSimulationIMU.cpp:94
iCubSimulationIMU::getNrOfThreeAxisMagnetometers
size_t getNrOfThreeAxisMagnetometers() const override
Definition: iCubSimulationIMU.cpp:160
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCubSimulationIMU::getNrOfThreeAxisGyroscopes
size_t getNrOfThreeAxisGyroscopes() const override
Definition: iCubSimulationIMU.cpp:84
iCubSimulationIMU::getThreeAxisLinearAccelerometerName
bool getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Definition: iCubSimulationIMU.cpp:132
iCubSimulationIMU::getOrientationSensorFrameName
bool getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Definition: iCubSimulationIMU.cpp:213
OdeInit::removeSimulationIMU
void removeSimulationIMU()
Definition: OdeInit.cpp:140
iCubSimulationIMU::getOrientationSensorName
bool getOrientationSensorName(size_t sens_index, std::string &name) const override
Definition: iCubSimulationIMU.cpp:208
iCubSimulationIMU::~iCubSimulationIMU
virtual ~iCubSimulationIMU()
Definition: iCubSimulationIMU.cpp:32
OdeInit::mtx
std::mutex mtx
Definition: OdeInit.h:65
OdeInit
ODE state information.
Definition: OdeInit.h:55