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 <limits>
28 #include <cstring>
29 #include <cstdlib>
30 
31 using 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 
94 /** Constructor.
95  * @param thread_name name of the thread, be descriptive
96  * @param continuous true to run continuous, false otherwise
97  * @param cfg_name configuration name
98  * @param cfg_prefix configuration path prefix
99  */
100 IMUAcquisitionThread::IMUAcquisitionThread(const char *thread_name, bool continuous,
101  std::string &cfg_name, std::string &cfg_prefix)
102  : Thread(thread_name, Thread::OPMODE_CONTINUOUS)
103 {
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) orientation_[i] = 0.;
113  for (unsigned int i = 0; i < 9; ++i) orientation_covariance_[i] = 0.;
114  for (unsigned int i = 0; i < 3; ++i) angular_velocity_[i] = 0.;
115  for (unsigned int i = 0; i < 9; ++i) angular_velocity_covariance_[i] = 0.;
116  for (unsigned int i = 0; i < 3; ++i) linear_acceleration_[i] = 0.;
117  for (unsigned int i = 0; i < 9; ++i) linear_acceleration_covariance_[i] = 0.;
118 
119 }
120 
121 IMUAcquisitionThread::~IMUAcquisitionThread()
122 {
123  delete data_mutex_;
124  delete timestamp_;
125 }
126 
127 
128 void
130 {
131  if (! cfg_continuous_) return;
132 
133  imu_if_ = NULL;
134  cfg_frame_ = config->get_string((cfg_prefix_ + "frame").c_str());
135 
136  std::string if_id = "IMU " + cfg_name_;
137 
138  imu_if_ = blackboard->open_for_writing<IMUInterface>(if_id.c_str());
139  imu_if_->set_auto_timestamping(false);
140  imu_if_->set_frame(cfg_frame_.c_str());
141  imu_if_->write();
142 }
143 
144 
145 void
147 {
148  blackboard->close(imu_if_);
149 }
150 
151 
152 void
154 {
155  data_mutex_->lock();
156  if (new_data_) {
157  imu_if_->set_timestamp(timestamp_);
158  imu_if_->set_orientation(orientation_);
164  imu_if_->write();
165  }
166  data_mutex_->unlock();
167 }
168 
169 /** Lock data if fresh.
170  * If new data has been received since get_distance_data() or get_echo_data()
171  * was called last the data is locked, no new data can arrive until you call
172  * unlock(), otherwise the lock is immediately released after checking.
173  * @return true if the lock was acquired and there is new data, false otherwise
174  */
175 bool
177 {
178  data_mutex_->lock();
179  if (new_data_) {
180  return true;
181  } else {
182  data_mutex_->unlock();
183  return false;
184  }
185 }
186 
187 
188 /** Unlock data, */
189 void
191 {
192  data_mutex_->unlock();
193 }
float angular_velocity_[3]
Pre-allocated angular velocities as array, 3 entries ordered (x,y,z).
void unlock()
Unlock data,.
double orientation_covariance_[9]
Pre-allocated orientation covariance, row major matrix ordered x, y, z.
float orientation_[4]
Pre-allocated orientation quaternion as array, 4 entries ordered (x,y,z,w).
virtual void init()
Initialize the thread.
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:760
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
std::string cfg_name_
Configuration name (third element in config path).
A class for handling time.
Definition: time.h:91
fawkes::Time * timestamp_
Time when the most recent data was received.
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance 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_frame(const char *new_frame)
Set frame value.
bool lock_if_new_data()
Lock data if fresh.
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
bool new_data_
Set to true in your loop if new data is available.
std::string cfg_frame_
Coordinate frame for sensor.
std::string cfg_prefix_
Configuration path prefix.
virtual void loop()
Code to execute in the thread.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
IMUAcquisitionThread(const char *thread_name, bool continuous, std::string &cfg_name, std::string &cfg_prefix)
Constructor.
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:33
double angular_velocity_covariance_[9]
Pre-allocated angular velocity covariance, row major matrix ordered x, y, z.
fawkes::Mutex * data_mutex_
Lock while writing to distances or echoes array or marking new data.
virtual void finalize()
Finalize the thread.
void lock()
Lock this mutex.
Definition: mutex.cpp:89
double linear_acceleration_covariance_[9]
Pre-allocated linear acceleration covariance, row major matrix ordered x, y, z.
bool cfg_continuous_
True if running continuous.
Mutex mutual exclusion lock.
Definition: mutex.h:32
float linear_acceleration_[3]
Pre-allocated linear acceleration as array, 3 entries ordered (x,y,z).
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
void set_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:729
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.