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 
25 #include "acquisition_thread.h"
26 
27 #include <interfaces/IMUInterface.h>
28 
29 using namespace fawkes;
30 
31 /** @class IMUSensorThread "sensor_thread.h"
32  * IMU sensor thread.
33  * This thread integrates into the Fawkes main loop at the sensor hook and
34  * publishes new data when available from the IMUAcquisitionThread.
35  * @author Tim Niemueller
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 void
57 {
58  imu_if_ = NULL;
59 
60  cfg_frame_ = config->get_string((cfg_prefix_ + "frame").c_str());
61 
62  std::string if_id = "IMU " + cfg_name_;
63 
64  imu_if_ = blackboard->open_for_writing<IMUInterface>(if_id.c_str());
65  imu_if_->set_auto_timestamping(false);
66  imu_if_->set_frame(cfg_frame_.c_str());
67  imu_if_->write();
68 }
69 
70 void
72 {
73  blackboard->close(imu_if_);
74 }
75 
76 void
78 {
79  if (aqt_->lock_if_new_data()) {
80  imu_if_->set_timestamp(aqt_->get_timestamp());
81  imu_if_->set_orientation(aqt_->get_orientation());
87  imu_if_->write();
88  aqt_->unlock();
89  }
90 }
IMU acqusition thread.
void unlock()
Unlock data,.
const double * get_orientation_covariance()
Get orientation covariance.
const float * get_orientation()
Get orientation data.
const double * get_linear_acceleration_covariance()
Get linera acceleration covariance.
const fawkes::Time * get_timestamp()
Get time of data set.
bool lock_if_new_data()
Lock data if fresh.
const float * get_linear_acceleration()
Get linear acceleration data.
const float * get_angular_velocity()
Get angular velocity data.
const double * get_angular_velocity_covariance()
Get angular velocity covariance.
IMUSensorThread(std::string &cfg_name, std::string &cfg_prefix, IMUAcquisitionThread *aqt)
Constructor.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
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.
Thread aspect to use blocked timing.
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:746
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
void set_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:715
Thread class encapsulation of pthreads.
Definition: thread.h:46
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Fawkes library namespace.