Fawkes API  Fawkes Development Version
sensor_thread.cpp
1 
2 /***************************************************************************
3  * sensor_thread.cpp - IMU thread that pushes data into the interface
4  *
5  * Created: Sun Jun 22 19:42:14 2014
6  * Copyright 2006-2014 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "sensor_thread.h"
24 #include "acquisition_thread.h"
25 
26 #include <interfaces/IMUInterface.h>
27 
28 using namespace fawkes;
29 
30 /** @class IMUSensorThread "sensor_thread.h"
31  * IMU sensor thread.
32  * This thread integrates into the Fawkes main loop at the sensor hook and
33  * publishes new data when available from the IMUAcquisitionThread.
34  * @author Tim Niemueller
35  */
36 
37 
38 /** Constructor.
39  * @param cfg_name short name of configuration group
40  * @param cfg_prefix configuration path prefix
41  * @param aqt IMUAcquisitionThread to get data from
42  */
43 IMUSensorThread::IMUSensorThread(std::string &cfg_name,
44  std::string &cfg_prefix,
46  : Thread("IMUSensorThread", Thread::OPMODE_WAITFORWAKEUP),
47  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
48 {
49  set_name("IMUSensorThread(%s)", cfg_name.c_str());
50  aqt_ = aqt;
51  cfg_name_ = cfg_name;
52  cfg_prefix_ = cfg_prefix;
53 }
54 
55 
56 void
58 {
59  imu_if_ = NULL;
60 
61  cfg_frame_ = config->get_string((cfg_prefix_ + "frame").c_str());
62 
63  std::string if_id = "IMU " + cfg_name_;
64 
65  imu_if_ = blackboard->open_for_writing<IMUInterface>(if_id.c_str());
66  imu_if_->set_auto_timestamping(false);
67  imu_if_->set_frame(cfg_frame_.c_str());
68  imu_if_->write();
69 }
70 
71 
72 void
74 {
75  blackboard->close(imu_if_);
76 }
77 
78 void
80 {
81  if (aqt_->lock_if_new_data()) {
82  imu_if_->set_timestamp(aqt_->get_timestamp());
83  imu_if_->set_orientation(aqt_->get_orientation());
89  imu_if_->write();
90  aqt_->unlock();
91  }
92 }
void unlock()
Unlock data,.
const double * get_orientation_covariance()
Get orientation covariance.
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.
const float * get_linear_acceleration()
Get linear acceleration data.
IMU acqusition thread.
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.
const float * get_angular_velocity()
Get angular velocity data.
const float * get_orientation()
Get orientation data.
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
Thread aspect to use blocked timing.
void set_frame(const char *new_frame)
Set frame value.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:761
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.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
const double * get_angular_velocity_covariance()
Get angular velocity covariance.
virtual void init()
Initialize the thread.
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:33
const double * get_linear_acceleration_covariance()
Get linera acceleration covariance.
const fawkes::Time * get_timestamp()
Get time of data set.
IMUSensorThread(std::string &cfg_name, std::string &cfg_prefix, IMUAcquisitionThread *aqt)
Constructor.
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.