Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * sensor_thread.cpp - Laser thread that puses data into the interface 00004 * 00005 * Created: Wed Oct 08 13:32:57 2008 00006 * Copyright 2006-2008 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "sensor_thread.h" 00024 #include "acquisition_thread.h" 00025 00026 #include <interfaces/Laser360Interface.h> 00027 #include <interfaces/Laser720Interface.h> 00028 00029 using namespace fawkes; 00030 00031 /** @class LaserSensorThread "sensor_thread.h" 00032 * Laser sensor thread. 00033 * This thread integrates into the Fawkes main loop at the sensor hook and 00034 * publishes new data when available from the LaserAcquisitionThread. 00035 * @author Tim Niemueller 00036 */ 00037 00038 00039 /** Constructor. 00040 * @param cfg_name short name of configuration group 00041 * @param cfg_prefix configuration path prefix 00042 * @param aqt LaserAcquisitionThread to get data from 00043 */ 00044 LaserSensorThread::LaserSensorThread(std::string &cfg_name, 00045 std::string &cfg_prefix, 00046 LaserAcquisitionThread *aqt) 00047 : Thread("LaserSensorThread", Thread::OPMODE_WAITFORWAKEUP), 00048 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE) 00049 { 00050 set_name("LaserSensorThread(%s)", cfg_name.c_str()); 00051 __aqt = aqt; 00052 __cfg_name = cfg_name; 00053 __cfg_prefix = cfg_prefix; 00054 } 00055 00056 00057 void 00058 LaserSensorThread::init() 00059 { 00060 __laser360_if = NULL; 00061 __laser720_if = NULL; 00062 00063 bool main_sensor = false; 00064 00065 __cfg_frame = config->get_string((__cfg_prefix + "frame").c_str()); 00066 00067 try { 00068 main_sensor = config->get_bool((__cfg_prefix + "main_sensor").c_str()); 00069 } catch (Exception &e) {} // ignored, assume no 00070 00071 __aqt->pre_init(config, logger); 00072 00073 __num_values = __aqt->get_distance_data_size(); 00074 00075 std::string if_id = main_sensor ? "Laser" : ("Laser " + __cfg_name); 00076 00077 if (__num_values == 360) { 00078 __laser360_if = blackboard->open_for_writing<Laser360Interface>(if_id.c_str()); 00079 __laser360_if->set_frame(__cfg_frame.c_str()); 00080 __laser360_if->write(); 00081 } else if (__num_values == 720){ 00082 __laser720_if = blackboard->open_for_writing<Laser720Interface>(if_id.c_str()); 00083 __laser720_if->set_frame(__cfg_frame.c_str()); 00084 __laser720_if->write(); 00085 } else { 00086 throw Exception("Laser acquisition thread must produce either 360 or 720 " 00087 "distance values, but it produces %u", __aqt->get_distance_data_size()); 00088 } 00089 00090 } 00091 00092 00093 void 00094 LaserSensorThread::finalize() 00095 { 00096 blackboard->close(__laser360_if); 00097 blackboard->close(__laser720_if); 00098 } 00099 00100 void 00101 LaserSensorThread::loop() 00102 { 00103 if ( __aqt->lock_if_new_data() ) { 00104 if (__num_values == 360) { 00105 __laser360_if->set_distances(__aqt->get_distance_data()); 00106 __laser360_if->write(); 00107 } else if (__num_values == 720) { 00108 __laser720_if->set_distances(__aqt->get_distance_data()); 00109 __laser720_if->write(); 00110 } 00111 __aqt->unlock(); 00112 } 00113 }