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 
29 using namespace fawkes;
30 
31 /** @class LaserSensorThread "sensor_thread.h"
32  * Laser sensor thread.
33  * This thread integrates into the Fawkes main loop at the sensor hook and
34  * publishes new data when available from the LaserAcquisitionThread.
35  * @author Tim Niemueller
36  */
37 
38 
39 /** Constructor.
40  * @param cfg_name short name of configuration group
41  * @param cfg_prefix configuration path prefix
42  * @param aqt LaserAcquisitionThread to get data from
43  */
45  std::string &cfg_prefix,
47  : Thread("LaserSensorThread", Thread::OPMODE_WAITFORWAKEUP),
48  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
49 {
50  set_name("LaserSensorThread(%s)", cfg_name.c_str());
51  __aqt = aqt;
52  __cfg_name = cfg_name;
53  __cfg_prefix = cfg_prefix;
54 }
55 
56 
57 void
59 {
60  __laser360_if = NULL;
61  __laser720_if = NULL;
62 
63  bool main_sensor = false;
64 
65  __cfg_frame = config->get_string((__cfg_prefix + "frame").c_str());
66 
67  try {
68  main_sensor = config->get_bool((__cfg_prefix + "main_sensor").c_str());
69  } catch (Exception &e) {} // ignored, assume no
70 
71  __aqt->pre_init(config, logger);
72 
73  __num_values = __aqt->get_distance_data_size();
74 
75  std::string if_id = main_sensor ? "Laser" : ("Laser " + __cfg_name);
76 
77  if (__num_values == 360) {
78  __laser360_if = blackboard->open_for_writing<Laser360Interface>(if_id.c_str());
79  __laser360_if->set_frame(__cfg_frame.c_str());
80  __laser360_if->write();
81  } else if (__num_values == 720){
82  __laser720_if = blackboard->open_for_writing<Laser720Interface>(if_id.c_str());
83  __laser720_if->set_frame(__cfg_frame.c_str());
84  __laser720_if->write();
85  } else {
86  throw Exception("Laser acquisition thread must produce either 360 or 720 "
87  "distance values, but it produces %u", __aqt->get_distance_data_size());
88  }
89 
90 }
91 
92 
93 void
95 {
96  blackboard->close(__laser360_if);
97  blackboard->close(__laser720_if);
98 }
99 
100 void
102 {
103  if ( __aqt->lock_if_new_data() ) {
104  if (__num_values == 360) {
105  __laser360_if->set_distances(__aqt->get_distance_data());
106  __laser360_if->write();
107  } else if (__num_values == 720) {
108  __laser720_if->set_distances(__aqt->get_distance_data());
109  __laser720_if->write();
110  }
111  __aqt->unlock();
112  }
113 }
Laser360Interface Fawkes BlackBoard Interface.
unsigned int get_distance_data_size()
Get distance data size.
void set_frame(const char *new_frame)
Set frame value.
Laser acqusition thread.
void set_frame(const char *new_frame)
Set frame value.
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:495
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
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
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:749
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.
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
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:43
virtual void close(Interface *interface)=0
Close interface.