Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * laserscan_thread.h - Thread to exchange laser scans 00004 * 00005 * Created: Tue May 29 19:32:39 2012 00006 * Copyright 2011-2012 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #ifndef __PLUGINS_ROS_LASERSCAN_THREAD_H_ 00023 #define __PLUGINS_ROS_LASERSCAN_THREAD_H_ 00024 00025 #include <core/threading/thread.h> 00026 #include <aspect/blocked_timing.h> 00027 #include <aspect/configurable.h> 00028 #include <aspect/logging.h> 00029 #include <aspect/blackboard.h> 00030 #include <plugins/ros/aspect/ros.h> 00031 #include <blackboard/interface_listener.h> 00032 #include <blackboard/interface_observer.h> 00033 #include <interfaces/Laser360Interface.h> 00034 #include <interfaces/Laser720Interface.h> 00035 #include <core/threading/mutex.h> 00036 #include <utils/time/time.h> 00037 00038 #include <list> 00039 #include <queue> 00040 00041 #include <ros/node_handle.h> 00042 #include <sensor_msgs/LaserScan.h> 00043 00044 class RosLaserScanThread 00045 : public fawkes::Thread, 00046 public fawkes::LoggingAspect, 00047 public fawkes::ConfigurableAspect, 00048 public fawkes::BlockedTimingAspect, 00049 public fawkes::BlackBoardAspect, 00050 public fawkes::ROSAspect, 00051 public fawkes::BlackBoardInterfaceObserver, 00052 public fawkes::BlackBoardInterfaceListener 00053 { 00054 public: 00055 RosLaserScanThread(); 00056 virtual ~RosLaserScanThread(); 00057 00058 virtual void init(); 00059 virtual void loop(); 00060 virtual void finalize(); 00061 00062 // for BlackBoardInterfaceObserver 00063 virtual void bb_interface_created(const char *type, const char *id) throw(); 00064 00065 // for BlackBoardInterfaceListener 00066 virtual void bb_interface_data_changed(fawkes::Interface *interface) throw(); 00067 virtual void bb_interface_writer_removed(fawkes::Interface *interface, 00068 unsigned int instance_serial) throw(); 00069 virtual void bb_interface_reader_removed(fawkes::Interface *interface, 00070 unsigned int instance_serial) throw(); 00071 00072 private: 00073 void laser_scan_message_cb(const sensor_msgs::LaserScan::ConstPtr &msg); 00074 void conditional_close(fawkes::Interface *interface) throw(); 00075 std::string topic_name(const char *if_id, const char *suffix); 00076 00077 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00078 protected: virtual void run() { Thread::run(); } 00079 00080 private: 00081 std::list<fawkes::Laser360Interface *> __ls360_ifs; 00082 std::list<fawkes::Laser720Interface *> __ls720_ifs; 00083 00084 ros::Subscriber __sub_ls; 00085 00086 /// @cond INTERNALS 00087 typedef struct { 00088 ros::Publisher pub; 00089 sensor_msgs::LaserScan msg; 00090 } PublisherInfo; 00091 /// @endcond 00092 std::map<std::string, PublisherInfo> __pubs; 00093 00094 fawkes::Mutex *__ls_msg_queue_mutex; 00095 unsigned int __active_queue; 00096 std::queue<sensor_msgs::LaserScan::ConstPtr> __ls_msg_queues[2]; 00097 00098 std::map<std::string, fawkes::Laser360Interface *> __ls360_wifs; 00099 00100 fawkes::Mutex *__seq_num_mutex; 00101 unsigned int __seq_num; 00102 00103 }; 00104 00105 #endif