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 #include "acquisition_thread.h"
25 
26 #include <interfaces/Laser360Interface.h>
27 #include <interfaces/Laser720Interface.h>
28 #include <interfaces/Laser1080Interface.h>
29 
30 using namespace fawkes;
31 
32 /** @class LaserSensorThread "sensor_thread.h"
33  * Laser sensor thread.
34  * This thread integrates into the Fawkes main loop at the sensor hook and
35  * publishes new data when available from the LaserAcquisitionThread.
36  * @author Tim Niemueller
37  */
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 
57 
58 void
60 {
61  __laser360_if = NULL;
62  __laser720_if = NULL;
63  __laser1080_if = NULL;
64 
65  bool main_sensor = false;
66 
67  __cfg_frame = config->get_string((__cfg_prefix + "frame").c_str());
68 
69  try {
70  main_sensor = config->get_bool((__cfg_prefix + "main_sensor").c_str());
71  } catch (Exception &e) {} // 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", __aqt->get_distance_data_size());
97  }
98 
99 }
100 
101 
102 void
104 {
105  blackboard->close(__laser360_if);
106  blackboard->close(__laser720_if);
107  blackboard->close(__laser1080_if);
108 }
109 
110 void
112 {
113  if ( __aqt->lock_if_new_data() ) {
114  if (__num_values == 360) {
115  __laser360_if->set_timestamp(__aqt->get_timestamp());
116  __laser360_if->set_distances(__aqt->get_distance_data());
117  __laser360_if->write();
118  } else if (__num_values == 720) {
119  __laser720_if->set_timestamp(__aqt->get_timestamp());
120  __laser720_if->set_distances(__aqt->get_distance_data());
121  __laser720_if->write();
122  } else if (__num_values == 1080) {
123  __laser1080_if->set_timestamp(__aqt->get_timestamp());
124  __laser1080_if->set_distances(__aqt->get_distance_data());
125  __laser1080_if->write();
126  }
127  __aqt->unlock();
128  }
129 }
Laser360Interface Fawkes BlackBoard Interface.
unsigned int get_distance_data_size()
Get distance data size.
void set_frame(const char *new_frame)
Set frame value.
Laser1080Interface Fawkes BlackBoard Interface.
Laser acqusition thread.
void set_frame(const char *new_frame)
Set frame value.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:760
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)=0
Pre initialization.
virtual void init()
Initialize the 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_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
LaserSensorThread(std::string &cfg_name, std::string &cfg_prefix, LaserAcquisitionThread *aqt)
Constructor.
Thread aspect to use blocked timing.
virtual void loop()
Code to execute in the thread.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:761
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void finalize()
Finalize the thread.
const float * get_distance_data()
Get distance data.
void unlock()
Unlock data,.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
const fawkes::Time * get_timestamp()
Get timestamp of data.
void set_frame(const char *new_frame)
Set frame value.
bool lock_if_new_data()
Lock data if fresh.
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_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:729
Laser720Interface Fawkes BlackBoard Interface.
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.