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
29using 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 */
43IMUSensorThread::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
55void
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
70void
72{
73 blackboard->close(imu_if_);
74}
75
76void
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.
const fawkes::Time * get_timestamp()
Get time of data set.
const float * get_angular_velocity()
Get angular velocity data.
const double * get_linear_acceleration_covariance()
Get linera acceleration covariance.
void unlock()
Unlock data,.
bool lock_if_new_data()
Lock data if fresh.
const float * get_orientation()
Get orientation data.
const double * get_angular_velocity_covariance()
Get angular velocity covariance.
const double * get_orientation_covariance()
Get orientation covariance.
const float * get_linear_acceleration()
Get linear acceleration data.
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: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
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.