Fawkes API Fawkes Development Version
sensor_thread.cpp
1
2/***************************************************************************
3 * sensor_thread.cpp - Laser thread that puses data into the interface
4 *
5 * Created: Wed Oct 08 13:32:57 2008
6 * Copyright 2006-2008 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/Laser1080Interface.h>
28#include <interfaces/Laser360Interface.h>
29#include <interfaces/Laser720Interface.h>
30
31using namespace fawkes;
32
33/** @class LaserSensorThread "sensor_thread.h"
34 * Laser sensor thread.
35 * This thread integrates into the Fawkes main loop at the sensor hook and
36 * publishes new data when available from the LaserAcquisitionThread.
37 * @author Tim Niemueller
38 */
39
40/** Constructor.
41 * @param cfg_name short name of configuration group
42 * @param cfg_prefix configuration path prefix
43 * @param aqt LaserAcquisitionThread to get data from
44 */
46 std::string & cfg_prefix,
48: Thread("LaserSensorThread", Thread::OPMODE_WAITFORWAKEUP),
49 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
50{
51 set_name("LaserSensorThread(%s)", cfg_name.c_str());
52 aqt_ = aqt;
53 cfg_name_ = cfg_name;
54 cfg_prefix_ = cfg_prefix;
55}
56
57void
59{
60 laser360_if_ = NULL;
61 laser720_if_ = NULL;
62 laser1080_if_ = NULL;
63
64 bool main_sensor = false;
65
66 cfg_frame_ = config->get_string((cfg_prefix_ + "frame").c_str());
67
68 try {
69 main_sensor = config->get_bool((cfg_prefix_ + "main_sensor").c_str());
70 } catch (Exception &e) {
71 } // ignored, assume no
72
73 aqt_->pre_init(config, logger);
74
75 num_values_ = aqt_->get_distance_data_size();
76
77 std::string if_id = main_sensor ? "Laser" : ("Laser " + cfg_name_);
78
79 if (num_values_ == 360) {
80 laser360_if_ = blackboard->open_for_writing<Laser360Interface>(if_id.c_str());
81 laser360_if_->set_auto_timestamping(false);
82 laser360_if_->set_frame(cfg_frame_.c_str());
83 laser360_if_->write();
84 } else if (num_values_ == 720) {
85 laser720_if_ = blackboard->open_for_writing<Laser720Interface>(if_id.c_str());
86 laser720_if_->set_auto_timestamping(false);
87 laser720_if_->set_frame(cfg_frame_.c_str());
88 laser720_if_->write();
89 } else if (num_values_ == 1080) {
90 laser1080_if_ = blackboard->open_for_writing<Laser1080Interface>(if_id.c_str());
91 laser1080_if_->set_auto_timestamping(false);
92 laser1080_if_->set_frame(cfg_frame_.c_str());
93 laser1080_if_->write();
94 } else {
95 throw Exception("Laser acquisition thread must produce either 360, 720, or 1080 "
96 "distance values, but it produces %u",
98 }
99}
100
101void
103{
104 blackboard->close(laser360_if_);
105 blackboard->close(laser720_if_);
106 blackboard->close(laser1080_if_);
107}
108
109void
111{
112 if (aqt_->lock_if_new_data()) {
113 if (num_values_ == 360) {
114 laser360_if_->set_timestamp(aqt_->get_timestamp());
115 laser360_if_->set_distances(aqt_->get_distance_data());
116 laser360_if_->write();
117 } else if (num_values_ == 720) {
118 laser720_if_->set_timestamp(aqt_->get_timestamp());
119 laser720_if_->set_distances(aqt_->get_distance_data());
120 laser720_if_->write();
121 } else if (num_values_ == 1080) {
122 laser1080_if_->set_timestamp(aqt_->get_timestamp());
123 laser1080_if_->set_distances(aqt_->get_distance_data());
124 laser1080_if_->write();
125 }
126 aqt_->unlock();
127 }
128}
Laser acqusition thread.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)=0
Pre initialization.
const fawkes::Time * get_timestamp()
Get timestamp of data.
const float * get_distance_data()
Get distance data.
void unlock()
Unlock data,.
unsigned int get_distance_data_size()
Get distance data size.
bool lock_if_new_data()
Lock data if fresh.
virtual void finalize()
Finalize the thread.
LaserSensorThread(std::string &cfg_name, std::string &cfg_prefix, LaserAcquisitionThread *aqt)
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize 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 bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
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
Laser1080Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Laser360Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Laser720Interface Fawkes BlackBoard Interface.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
void set_frame(const char *new_frame)
Set frame value.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
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.