YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeIMU.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#include "FakeIMU.h"
7
8#include <yarp/os/Thread.h>
9#include <yarp/os/Time.h>
10#include <yarp/os/Semaphore.h>
11#include <yarp/os/Stamp.h>
13#include <yarp/os/LogStream.h>
14
15#include <string>
16
17using namespace yarp::os;
18using namespace yarp::dev;
19using namespace yarp::sig;
20using namespace yarp::math;
21
22namespace {
23YARP_LOG_COMPONENT(FAKEIMU, "yarp.device.fakeIMU")
24constexpr double DEFAULT_PERIOD = 0.01; // seconds
27
28constexpr double EARTH_GRAVITY = -9.81;
29}
30
37 rpy({0.0, 0.0, 0.0}),
38 gravity({0.0, 0.0, EARTH_GRAVITY, 0.0}),
39 dcm(4, 4),
40 accels({0.0, 0.0, 0.0, 0.0}),
41 nchannels(DEFAULT_NCHANNELS),
42 dummy_value(DEFAULT_DUMMY_VALUE)
43{
44 dcm.zero();
45}
46
48{
49 close();
50}
51
53{
54 if (!this->parseParams(config)) {return false;}
55
56 double dperiod = m_period / 1000.0;
58
59 start();
60 return true;
61}
62
64{
66 return true;
67}
68
69bool FakeIMU::threadInit()
70{
71 lastStamp.update();
72 return true;
73}
74
75void FakeIMU::run()
76{
77 static double count=10;
78
79 rpy[0] = 0;
80 rpy[1] = count * 3.14/180;
81 rpy[2] = 0;
82
83 dcm = rpy2dcm(rpy);
84 accels = gravity * dcm;
85
86 lastStamp.update();
87
88 dummy_value = count;
89 if (!m_constantValue) {
90 count++;
91 }
92
93 if (count >= 360) {
94 count = 0;
95 }
96}
97
98yarp::dev::MAS_status FakeIMU::genericGetStatus(size_t sens_index) const
99{
100 if (sens_index!=0) {
102 }
103
105}
106
107ReturnValue FakeIMU::genericGetSensorName(size_t sens_index, std::string &name) const
108{
109 if (sens_index!=0) {
111 }
112
113 name = m_sensorName;
114 return ReturnValue_ok;
115}
116
117ReturnValue FakeIMU::genericGetFrameName(size_t sens_index, std::string &frameName) const
118{
119 if (sens_index!=0) {
121 }
122
123 frameName = m_frameName;
124 return ReturnValue_ok;
125}
126
128{
129 num =1;
130 return ReturnValue_ok;
131}
132
134{
135 return genericGetStatus(sens_index);
136}
137
139{
140 return genericGetSensorName(sens_index, name);
141}
142
144{
145 return genericGetFrameName(sens_index, frameName);
146}
147
149{
150 if (sens_index!=0) {
152 }
153
154 out.resize(3);
155 out[0] = dummy_value;
156 out[1] = dummy_value;
157 out[2] = dummy_value;
158
159 // Workaround for https://github.com/robotology/yarp/issues/1610
160 yarp::os::Stamp copyStamp(lastStamp);
161 timestamp = copyStamp.getTime();
162
163 return ReturnValue_ok;
164}
165
167{
168 num = 1;
169 return ReturnValue_ok;
170}
171
176
178{
179 return genericGetSensorName(sens_index, name);
180}
181
183{
184 return genericGetFrameName(sens_index, frameName);
185}
186
188{
189 if (sens_index!=0) {
191 }
192
193 out.resize(3);
194 out[0] = accels[0];
195 out[1] = accels[1];
196 out[2] = accels[2];
197
198 // Workaround for https://github.com/robotology/yarp/issues/1610
199 yarp::os::Stamp copyStamp(lastStamp);
200 timestamp = copyStamp.getTime();
201
202 return ReturnValue_ok;
203}
204
206{
207 num = 1;
208 return ReturnValue_ok;
209}
210
212{
213 return genericGetStatus(sens_index);
214}
215
217{
218 return genericGetSensorName(sens_index, name);
219}
220
222{
223 return genericGetFrameName(sens_index, frameName);
224}
225
227{
228 if (sens_index!=0) {
230 }
231
232 out.resize(3);
233 out[0] = dummy_value;
234 out[1] = dummy_value;
235 out[2] = dummy_value;
236
237 // Workaround for https://github.com/robotology/yarp/issues/1610
238 yarp::os::Stamp copyStamp(lastStamp);
239 timestamp = copyStamp.getTime();
240
241 return ReturnValue_ok;
242}
243
245{
246 num = 1;
247 return ReturnValue_ok;
248}
249
251{
252 return genericGetStatus(sens_index);
253}
254
256{
257 return genericGetSensorName(sens_index, name);
258}
259
261{
262 return genericGetFrameName(sens_index, frameName);
263}
264
266{
267 if (sens_index!=0) {
269 }
270
271 rpy_out.resize(3);
272 rpy_out[0] = dummy_value;
273 rpy_out[1] = dummy_value;
274 rpy_out[2] = dummy_value;
275
276 // Workaround for https://github.com/robotology/yarp/issues/1610
277 yarp::os::Stamp copyStamp(lastStamp);
278 timestamp = copyStamp.getTime();
279
280 return ReturnValue_ok;
281}
const double DEFAULT_PERIOD
#define ReturnValue_ok
Definition ReturnValue.h:82
#define ReturnValue_error_generic
Definition ReturnValue.h:83
#define ReturnValue_error_input_out_of_bounds
Definition ReturnValue.h:89
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
fakeIMU : fake device implementing the device interface typically implemented by an Inertial Measurem...
Definition FakeIMU.h:29
yarp::dev::MAS_status getThreeAxisGyroscopeStatus(size_t sens_index) const override
Get the status of the specified sensor.
Definition FakeIMU.cpp:133
yarp::dev::ReturnValue getNrOfThreeAxisMagnetometers(size_t &num) const override
Get the number of magnetometers exposed by this device.
Definition FakeIMU.cpp:205
~FakeIMU() override
Definition FakeIMU.cpp:47
yarp::dev::ReturnValue getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
Definition FakeIMU.cpp:177
yarp::dev::ReturnValue getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
Definition FakeIMU.cpp:182
yarp::dev::ReturnValue getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
Definition FakeIMU.cpp:260
yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
Definition FakeIMU.cpp:172
yarp::dev::ReturnValue getNrOfOrientationSensors(size_t &num) const override
Get the number of orientation sensors exposed by this device.
Definition FakeIMU.cpp:244
yarp::dev::ReturnValue getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
Definition FakeIMU.cpp:226
yarp::dev::ReturnValue getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
Definition FakeIMU.cpp:138
yarp::dev::ReturnValue getNrOfThreeAxisLinearAccelerometers(size_t &num) const override
Get the number of three axis linear accelerometers exposed by this device.
Definition FakeIMU.cpp:166
yarp::dev::ReturnValue getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
Definition FakeIMU.cpp:216
yarp::dev::ReturnValue getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
Definition FakeIMU.cpp:221
yarp::dev::ReturnValue getNrOfThreeAxisGyroscopes(size_t &num) const override
Get the number of three axis gyroscopes exposed by this sensor.
Definition FakeIMU.cpp:127
yarp::dev::ReturnValue getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
Definition FakeIMU.cpp:143
bool close() override
Close the DeviceDriver.
Definition FakeIMU.cpp:63
yarp::dev::ReturnValue getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the gyroscope.
Definition FakeIMU.cpp:148
yarp::dev::ReturnValue getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector &out, double &timestamp) const override
Get the last reading of the specified sensor.
Definition FakeIMU.cpp:187
yarp::dev::ReturnValue getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &rpy, double &timestamp) const override
Get the last reading of the orientation sensor as roll pitch yaw.
Definition FakeIMU.cpp:265
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
Definition FakeIMU.cpp:52
yarp::dev::ReturnValue getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
Definition FakeIMU.cpp:255
yarp::dev::MAS_status getThreeAxisMagnetometerStatus(size_t sens_index) const override
Get the status of the specified sensor.
Definition FakeIMU.cpp:211
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
Definition FakeIMU.cpp:250
A mini-server for performing network communication in the background.
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A base class for nested structures that can be searched.
Definition Searchable.h:31
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:18
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:49
void resize(size_t size)
Resize the vector.
Definition VectorBase.h:106
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
MAS_status
Status of a given analog sensor exposed by a multiple analog sensors interface.
@ MAS_ERROR
The sensor is in generic error state.
@ MAS_OK
The sensor is working correctly.
yarp::sig::Matrix rpy2dcm(const yarp::sig::Vector &rpy)
Converts roll-pitch-yaw angles in the corresponding dcm (direction cosine matrix) rotation matrix (de...
Definition math.cpp:847
An interface to the operating system, including Port based communication.