YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakePositionSensor.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
7
8#include <yarp/os/Time.h>
10#include <yarp/os/LogStream.h>
11
12using namespace yarp::dev;
13
14namespace {
15YARP_LOG_COMPONENT(FAKE_POSITION_SENSOR, "yarp.device.fakePositionSensor")
16}
17
18FakePositionSensor::FakePositionSensor(double period) : PeriodicThread(period),
19 m_mutex(),
20 m_channelsNum(1)
21{
22 yCTrace(FAKE_POSITION_SENSOR);
23}
24
26{
27 yCTrace(FAKE_POSITION_SENSOR);
28}
29
31{
32 if (!this->parseParams(config)) {return false;}
33
35
36 //create the data vector:
37 this->m_channelsNum = 1;
38 m_orientation_sensors.resize(m_channelsNum);
39 m_position_sensors.resize(m_channelsNum);
40
41 return PeriodicThread::start();
42}
43
45{
46 yCTrace(FAKE_POSITION_SENSOR);
47 //stop the thread
48 PeriodicThread::stop();
49
50 return true;
51}
52
54{
55 yCTrace(FAKE_POSITION_SENSOR);
56 return true;
57}
58
60{
61 m_mutex.lock();
62
63 // Do fake stuff
64 double timeNow = yarp::os::Time::now();
65
66 for (size_t i = 0; i < m_position_sensors.size(); i++)
67 {
68 m_position_sensors[i].m_timestamp = timeNow;
69 m_position_sensors[i].m_status = yarp::dev::MAS_status::MAS_OK;
70 for (auto it= m_position_sensors[i].m_data.begin(); it != m_position_sensors[i].m_data.end(); it++)
71 {
72 *it = *it + 0.001;
73 }
74 }
75 for (size_t i = 0; i < m_orientation_sensors.size(); i++)
76 {
77 m_orientation_sensors[i].m_timestamp = timeNow;
78 m_orientation_sensors[i].m_status = yarp::dev::MAS_status::MAS_OK;
79 for (auto it = m_orientation_sensors[i].m_data.begin(); it != m_orientation_sensors[i].m_data.end(); it++)
80 {
81 *it = *it - 0.001;
82 }
83 }
84
85 m_mutex.unlock();
86}
87
89{
90 yCTrace(FAKE_POSITION_SENSOR);
91}
92
93
95{
96 std::lock_guard<std::mutex> myLockGuard(m_mutex);
97 num = m_position_sensors.size();
98 return ReturnValue_ok;
99}
100
102{
103 std::lock_guard<std::mutex> myLockGuard (m_mutex);
104 if (sens_index >= m_position_sensors.size()) return yarp::dev::MAS_status::MAS_UNKNOWN;
105 return m_position_sensors[sens_index].m_status;
106}
107
108ReturnValue FakePositionSensor::getPositionSensorName(size_t sens_index, std::string& name) const
109{
110 std::lock_guard<std::mutex> myLockGuard(m_mutex);
111 if (sens_index >= m_position_sensors.size()) {return ReturnValue_error_input_out_of_bounds;}
112 name = m_position_sensors[sens_index].m_name;
113 return ReturnValue_ok;
114}
115
116ReturnValue FakePositionSensor::getPositionSensorFrameName(size_t sens_index, std::string& frameName) const
117{
118 std::lock_guard<std::mutex> myLockGuard(m_mutex);
119 if (sens_index >= m_position_sensors.size()) {return ReturnValue_error_input_out_of_bounds;}
120 frameName = m_position_sensors[sens_index].m_framename;
121 return ReturnValue_ok;
122}
123
124ReturnValue FakePositionSensor::getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector& xyz, double& timestamp) const
125{
126 std::lock_guard<std::mutex> myLockGuard(m_mutex);
127 if (sens_index >= m_position_sensors.size()) {return ReturnValue_error_input_out_of_bounds;}
128 timestamp = m_position_sensors[sens_index].m_timestamp;
129 xyz = m_position_sensors[sens_index].m_data;
130 return ReturnValue_ok;
131}
132
134{
135 std::lock_guard<std::mutex> myLockGuard(m_mutex);
136 num = m_orientation_sensors.size();
137 return ReturnValue_ok;
138}
139
141{
142 std::lock_guard<std::mutex> myLockGuard(m_mutex);
143 if (sens_index >= m_orientation_sensors.size()) return yarp::dev::MAS_status::MAS_UNKNOWN;
144 return m_orientation_sensors[sens_index].m_status;
145}
146
147ReturnValue FakePositionSensor::getOrientationSensorName(size_t sens_index, std::string& name) const
148{
149 std::lock_guard<std::mutex> myLockGuard(m_mutex);
150 if (sens_index >= m_orientation_sensors.size()) {return ReturnValue_error_input_out_of_bounds;}
151 name = m_orientation_sensors[sens_index].m_name;
152 return ReturnValue_ok;
153}
154
155ReturnValue FakePositionSensor::getOrientationSensorFrameName(size_t sens_index, std::string& frameName) const
156{
157 std::lock_guard<std::mutex> myLockGuard(m_mutex);
158 if (sens_index >= m_orientation_sensors.size()) {return ReturnValue_error_input_out_of_bounds;}
159 frameName = m_orientation_sensors[sens_index].m_framename;
160 return ReturnValue_ok;
161}
162
164{
165 std::lock_guard<std::mutex> myLockGuard(m_mutex);
166 if (sens_index >= m_orientation_sensors.size()) {return ReturnValue_error_input_out_of_bounds;}
167 timestamp = m_orientation_sensors[sens_index].m_timestamp;
168 xyz = m_orientation_sensors[sens_index].m_data;
169 return ReturnValue_ok;
170}
#define ReturnValue_ok
Definition ReturnValue.h:82
#define ReturnValue_error_input_out_of_bounds
Definition ReturnValue.h:89
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
FakePositionSensor(double period=0.05)
bool close() override
Close the DeviceDriver.
bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
yarp::dev::MAS_status getPositionSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
yarp::dev::MAS_status getOrientationSensorStatus(size_t sens_index) const override
Get the status of the specified sensor.
void run() override
Loop function.
yarp::dev::ReturnValue getOrientationSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
yarp::dev::ReturnValue getNrOfOrientationSensors(size_t &num) const override
Get the number of orientation sensors exposed by this device.
yarp::dev::ReturnValue getPositionSensorName(size_t sens_index, std::string &name) const override
Get the name of the specified sensor.
bool threadInit() override
Initialization method.
yarp::dev::ReturnValue getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector &xyz, double &timestamp) const override
Get the last reading of the orientation sensor as roll pitch yaw.
yarp::dev::ReturnValue getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
void threadRelease() override
Release method.
yarp::dev::ReturnValue getPositionSensorMeasure(size_t sens_index, yarp::sig::Vector &xyz, double &timestamp) const override
Get the last reading of the position sensor as x y z.
yarp::dev::ReturnValue getPositionSensorFrameName(size_t sens_index, std::string &frameName) const override
Get the name of the frame of the specified sensor.
yarp::dev::ReturnValue getNrOfPositionSensors(size_t &num) const override
Get the number of position sensors exposed by this device.
bool setPeriod(double period)
Set the (new) period of the thread.
A base class for nested structures that can be searched.
Definition Searchable.h:31
#define yCTrace(component,...)
#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_UNKNOWN
The sensor is in an unknown state.
@ MAS_OK
The sensor is working correctly.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121