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 }
fawkes::IMUInterface::set_orientation_covariance
void set_orientation_covariance(unsigned int index, const double new_orientation_covariance)
Set orientation_covariance value at given index.
Definition: IMUInterface.cpp:245
fawkes::IMUInterface::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: IMUInterface.cpp:118
fawkes::IMUInterface::set_angular_velocity_covariance
void set_angular_velocity_covariance(unsigned int index, const double new_angular_velocity_covariance)
Set angular_velocity_covariance value at given index.
Definition: IMUInterface.cpp:371
fawkes::IMUInterface
IMUInterface Fawkes BlackBoard Interface.
Definition: IMUInterface.h:34
IMUSensorThread::IMUSensorThread
IMUSensorThread(std::string &cfg_name, std::string &cfg_prefix, IMUAcquisitionThread *aqt)
Constructor.
Definition: sensor_thread.cpp:43
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
IMUAcquisitionThread::get_linear_acceleration
const float * get_linear_acceleration()
Get linear acceleration data.
Definition: acquisition_thread.h:95
fawkes::Interface::set_timestamp
void set_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:715
IMUAcquisitionThread
IMU acqusition thread.
Definition: acquisition_thread.h:44
IMUAcquisitionThread::get_orientation_covariance
const double * get_orientation_covariance()
Get orientation covariance.
Definition: acquisition_thread.h:71
fawkes::IMUInterface::set_linear_acceleration
void set_linear_acceleration(unsigned int index, const float new_linear_acceleration)
Set linear_acceleration value at given index.
Definition: IMUInterface.cpp:434
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes
Fawkes library namespace.
IMUSensorThread::finalize
virtual void finalize()
Finalize the thread.
Definition: sensor_thread.cpp:71
IMUAcquisitionThread::get_orientation
const float * get_orientation()
Get orientation data.
Definition: acquisition_thread.h:63
fawkes::IMUInterface::set_orientation
void set_orientation(unsigned int index, const float new_orientation)
Set orientation value at given index.
Definition: IMUInterface.cpp:182
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
IMUAcquisitionThread::get_timestamp
const fawkes::Time * get_timestamp()
Get time of data set.
Definition: acquisition_thread.h:111
fawkes::Interface::set_auto_timestamping
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:746
IMUAcquisitionThread::lock_if_new_data
bool lock_if_new_data()
Lock data if fresh.
Definition: acquisition_thread.cpp:179
IMUSensorThread::loop
virtual void loop()
Code to execute in the thread.
Definition: sensor_thread.cpp:77
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
IMUAcquisitionThread::get_angular_velocity_covariance
const double * get_angular_velocity_covariance()
Get angular velocity covariance.
Definition: acquisition_thread.h:87
IMUAcquisitionThread::get_angular_velocity
const float * get_angular_velocity()
Get angular velocity data.
Definition: acquisition_thread.h:79
fawkes::IMUInterface::set_linear_acceleration_covariance
void set_linear_acceleration_covariance(unsigned int index, const double new_linear_acceleration_covariance)
Set linear_acceleration_covariance value at given index.
Definition: IMUInterface.cpp:497
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
IMUAcquisitionThread::unlock
void unlock()
Unlock data,.
Definition: acquisition_thread.cpp:192
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
IMUAcquisitionThread::get_linear_acceleration_covariance
const double * get_linear_acceleration_covariance()
Get linera acceleration covariance.
Definition: acquisition_thread.h:103
fawkes::Thread::set_name
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
fawkes::IMUInterface::set_angular_velocity
void set_angular_velocity(unsigned int index, const float new_angular_velocity)
Set angular_velocity value at given index.
Definition: IMUInterface.cpp:308
IMUSensorThread::init
virtual void init()
Initialize the thread.
Definition: sensor_thread.cpp:56