Fawkes API Fawkes Development Version
acquisition_thread.cpp
1
2/***************************************************************************
3 * acqusition_thread.cpp - Thread that retrieves IMU data
4 *
5 * Created: Sun Jun 22 21:18:41 2014
6 * Copyright 2008-2014 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "acquisition_thread.h"
23
24#include <core/threading/mutex.h>
25#include <interfaces/IMUInterface.h>
26
27#include <cstdlib>
28#include <cstring>
29#include <limits>
30
31using namespace fawkes;
32
33/** @class IMUAcquisitionThread "acquisition_thread.h"
34 * IMU acqusition thread.
35 * Interface for different laser types.
36 * @author Tim Niemueller
37 */
38
39/** @var fawkes::Mutex * IMUAcquisitionThread::data_mutex_
40 * Lock while writing to distances or echoes array or marking new data
41 */
42
43/** @var bool IMUAcquisitionThread::new_data_
44 * Set to true in your loop if new data is available. Set to false automatically
45 * in get_distance_data() and get_echoes_data().
46 */
47
48/** @var float * IMUAcquisitionThread::orientation_
49 * Pre-allocated orientation quaternion as array, 4 entries ordered (x,y,z,w).
50 */
51
52/** @var float * IMUAcquisitionThread::orientation_covariance_
53 * Pre-allocated orientation covariance, row major matrix ordered x, y, z.
54 */
55
56/** @var float * IMUAcquisitionThread::angular_velocity_
57 * Pre-allocated angular velocities as array, 3 entries ordered (x,y,z).
58 */
59
60/** @var float * IMUAcquisitionThread::angular_velocity_covariance_
61 * Pre-allocated angular velocity covariance, row major matrix ordered x, y, z.
62 */
63
64/** @var float * IMUAcquisitionThread::linear_acceleration_
65 * Pre-allocated linear acceleration as array, 3 entries ordered (x,y,z).
66 */
67
68/** @var float * IMUAcquisitionThread::linear_acceleration_covariance_
69 * Pre-allocated linear acceleration covariance, row major matrix ordered x, y, z.
70 */
71
72/** @var fawkes::Time * IMUAcquisitionThread::timestamp_
73 * Time when the most recent data was received.
74 */
75
76/** @var std::string IMUAcquisitionThread::cfg_name_
77 * Configuration name (third element in config path).
78 */
79
80/** @var std::string IMUAcquisitionThread::cfg_prefix_
81 * Configuration path prefix.
82 */
83
84/** @var std::string IMUAcquisitionThread::cfg_frame_
85 * Coordinate frame for sensor.
86 */
87
88/** @var std::string IMUAcquisitionThread::cfg_continuous_
89 * True if running continuous.
90 * Sub-classes must call init(), loop(), and finalize().
91 */
92
93/** Constructor.
94 * @param thread_name name of the thread, be descriptive
95 * @param continuous true to run continuous, false otherwise
96 * @param cfg_name configuration name
97 * @param cfg_prefix configuration path prefix
98 */
100 bool continuous,
101 const std::string &cfg_name,
102 const std::string &cfg_prefix)
103: Thread(thread_name, Thread::OPMODE_CONTINUOUS),
104 cfg_name_(cfg_name),
105 cfg_prefix_(cfg_prefix),
106 cfg_continuous_(continuous)
107{
108 data_mutex_ = new Mutex();
109 timestamp_ = new Time();
110 new_data_ = false;
111
112 for (unsigned int i = 0; i < 4; ++i)
113 orientation_[i] = 0.;
114 for (unsigned int i = 0; i < 9; ++i)
116 for (unsigned int i = 0; i < 3; ++i)
117 angular_velocity_[i] = 0.;
118 for (unsigned int i = 0; i < 9; ++i)
120 for (unsigned int i = 0; i < 3; ++i)
121 linear_acceleration_[i] = 0.;
122 for (unsigned int i = 0; i < 9; ++i)
124}
125
126IMUAcquisitionThread::~IMUAcquisitionThread()
127{
128 delete data_mutex_;
129 delete timestamp_;
130}
131
132void
134{
135 if (!cfg_continuous_)
136 return;
137
138 imu_if_ = NULL;
139 cfg_frame_ = config->get_string((cfg_prefix_ + "frame").c_str());
140
141 std::string if_id = "IMU " + cfg_name_;
142
143 imu_if_ = blackboard->open_for_writing<IMUInterface>(if_id.c_str());
144 imu_if_->set_auto_timestamping(false);
145 imu_if_->set_frame(cfg_frame_.c_str());
146 imu_if_->write();
147}
148
149void
151{
152 blackboard->close(imu_if_);
153}
154
155void
157{
158 data_mutex_->lock();
159 if (new_data_) {
160 imu_if_->set_timestamp(timestamp_);
167 imu_if_->write();
168 }
170}
171
172/** Lock data if fresh.
173 * If new data has been received since get_distance_data() or get_echo_data()
174 * was called last the data is locked, no new data can arrive until you call
175 * unlock(), otherwise the lock is immediately released after checking.
176 * @return true if the lock was acquired and there is new data, false otherwise
177 */
178bool
180{
181 data_mutex_->lock();
182 if (new_data_) {
183 return true;
184 } else {
186 return false;
187 }
188}
189
190/** Unlock data, */
191void
193{
195}
float linear_acceleration_[3]
Pre-allocated linear acceleration as array, 3 entries ordered (x,y,z).
fawkes::Mutex * data_mutex_
Lock while writing to distances or echoes array or marking new data.
std::string cfg_frame_
Coordinate frame for sensor.
void unlock()
Unlock data,.
double linear_acceleration_covariance_[9]
Pre-allocated linear acceleration covariance, row major matrix ordered x, y, z.
IMUAcquisitionThread(const char *thread_name, bool continuous, const std::string &cfg_name, const std::string &cfg_prefix)
Constructor.
double angular_velocity_covariance_[9]
Pre-allocated angular velocity covariance, row major matrix ordered x, y, z.
double orientation_covariance_[9]
Pre-allocated orientation covariance, row major matrix ordered x, y, z.
virtual void loop()
Code to execute in the thread.
float orientation_[4]
Pre-allocated orientation quaternion as array, 4 entries ordered (x,y,z,w).
bool lock_if_new_data()
Lock data if fresh.
virtual void finalize()
Finalize the thread.
std::string cfg_prefix_
Configuration path prefix.
bool cfg_continuous_
True if running continuous.
std::string cfg_name_
Configuration name (third element in config path).
float angular_velocity_[3]
Pre-allocated angular velocities as array, 3 entries ordered (x,y,z).
bool new_data_
Set to true in your loop if new data is available.
virtual void init()
Initialize the thread.
fawkes::Time * timestamp_
Time when the most recent data was received.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
void set_frame(const char *new_frame)
Set frame value.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance value at given index.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:755
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
void set_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:724
Mutex mutual exclusion lock.
Definition: mutex.h:33
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
Thread class encapsulation of pthreads.
Definition: thread.h:46
A class for handling time.
Definition: time.h:93
Fawkes library namespace.