Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * rx28_thread.h - RX28 pan/tilt unit act thread 00004 * 00005 * Created: Thu Jun 18 09:52:16 2009 00006 * Copyright 2006-2011 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 #ifndef __PLUGINS_PANTILT_ROBOTIS_RX28_THREAD_H_ 00024 #define __PLUGINS_PANTILT_ROBOTIS_RX28_THREAD_H_ 00025 00026 #include "../act_thread.h" 00027 00028 #ifdef HAVE_TF 00029 # include <aspect/tf.h> 00030 #endif 00031 #include <blackboard/interface_listener.h> 00032 #include <utils/time/time.h> 00033 00034 #ifdef USE_TIMETRACKER 00035 # include <utils/time/tracker.h> 00036 #endif 00037 #include <string> 00038 #include <memory> 00039 00040 namespace fawkes { 00041 class PanTiltInterface; 00042 class LedInterface; 00043 class ReadWriteLock; 00044 class WaitCondition; 00045 } 00046 00047 class RobotisRX28; 00048 00049 class PanTiltRX28Thread 00050 : public PanTiltActThread, 00051 #ifdef HAVE_TF 00052 public fawkes::TransformAspect, 00053 #endif 00054 public fawkes::BlackBoardInterfaceListener 00055 { 00056 public: 00057 PanTiltRX28Thread(std::string &pantilt_cfg_prefix, 00058 std::string &ptu_cfg_prefix, 00059 std::string &ptu_name); 00060 00061 virtual void init(); 00062 virtual bool prepare_finalize_user(); 00063 virtual void finalize(); 00064 virtual void loop(); 00065 00066 // For BlackBoardInterfaceListener 00067 virtual bool bb_interface_message_received(fawkes::Interface *interface, 00068 fawkes::Message *message) throw(); 00069 00070 void update_sensor_values(); 00071 00072 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00073 protected: virtual void run() { Thread::run(); } 00074 00075 private: 00076 fawkes::PanTiltInterface *__pantilt_if; 00077 fawkes::LedInterface *__led_if; 00078 00079 fawkes::RefPtr<RobotisRX28> __rx28; 00080 00081 std::string __pantilt_cfg_prefix; 00082 std::string __ptu_cfg_prefix; 00083 std::string __ptu_name; 00084 std::string __cfg_device; 00085 unsigned int __cfg_read_timeout_ms; 00086 unsigned int __cfg_disc_timeout_ms; 00087 unsigned int __cfg_pan_servo_id; 00088 unsigned int __cfg_tilt_servo_id; 00089 bool __cfg_goto_zero_start; 00090 bool __cfg_turn_off; 00091 unsigned int __cfg_cw_compl_margin; 00092 unsigned int __cfg_ccw_compl_margin; 00093 unsigned int __cfg_cw_compl_slope; 00094 unsigned int __cfg_ccw_compl_slope; 00095 float __cfg_pan_min; 00096 float __cfg_pan_max; 00097 float __cfg_tilt_min; 00098 float __cfg_tilt_max; 00099 float __cfg_pan_margin; 00100 float __cfg_tilt_margin; 00101 float __cfg_pan_offset; 00102 float __cfg_tilt_offset; 00103 float __cfg_pan_start; 00104 float __cfg_tilt_start; 00105 #ifdef HAVE_TF 00106 std::string __cfg_base_frame; 00107 std::string __cfg_pan_link; 00108 std::string __cfg_tilt_link; 00109 00110 fawkes::tf::Vector3 __translation_pan; 00111 fawkes::tf::Vector3 __translation_tilt; 00112 #endif 00113 00114 float __last_pan; 00115 float __last_tilt; 00116 00117 class WorkerThread : public fawkes::Thread 00118 { 00119 public: 00120 WorkerThread(std::string ptu_name, fawkes::Logger *logger, 00121 fawkes::RefPtr<RobotisRX28> rx28, 00122 unsigned char pan_servo_id, unsigned char tilt_servo_id, 00123 float &pan_min, float &pan_max, float &tilt_min, float &tilt_max, 00124 float &pan_offset, float &tilt_offset); 00125 00126 ~WorkerThread(); 00127 void goto_pantilt(float pan, float tilt); 00128 void goto_pantilt_timed(float pan, float tilt, float time_sec); 00129 void get_pantilt(float &pan, float &tilt); 00130 void get_pantilt(float &pan, float &tilt, fawkes::Time &time); 00131 void set_velocities(float pan_vel, float tilt_vel); 00132 void get_velocities(float &pan_vel, float &tilt_vel); 00133 void set_margins(float pan_margin, float tilt_margin); 00134 bool is_final(); 00135 bool is_enabled(); 00136 void set_enabled(bool enabled); 00137 void set_led_enabled(bool enabled); 00138 void stop_motion(); 00139 bool has_fresh_data(); 00140 void wait_for_fresh_data(); 00141 00142 virtual void loop(); 00143 00144 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */ 00145 protected: virtual void run() { Thread::run(); } 00146 00147 private: 00148 void exec_goto_pantilt(float pan, float tilt); 00149 00150 private: 00151 fawkes::ReadWriteLock *__rx28_rwlock; 00152 fawkes::RefPtr<RobotisRX28> __rx28; 00153 fawkes::Logger *__logger; 00154 fawkes::WaitCondition *__update_waitcond; 00155 00156 unsigned char __pan_servo_id; 00157 unsigned char __tilt_servo_id; 00158 00159 float __pan_min; 00160 float __pan_max; 00161 float __tilt_min; 00162 float __tilt_max; 00163 float __pan_offset; 00164 float __tilt_offset; 00165 float __max_pan_speed; 00166 float __max_tilt_speed; 00167 float __pan_margin; 00168 float __tilt_margin; 00169 00170 fawkes::ReadWriteLock *__value_rwlock; 00171 bool __move_pending; 00172 float __target_pan; 00173 float __target_tilt; 00174 bool __enable; 00175 bool __disable; 00176 bool __velo_pending; 00177 unsigned int __pan_vel; 00178 unsigned int __tilt_vel; 00179 bool __led_enable; 00180 bool __led_disable; 00181 fawkes::Time __pantilt_time; 00182 00183 bool __fresh_data; 00184 fawkes::Mutex *__fresh_data_mutex; 00185 00186 }; 00187 00188 WorkerThread *__wt; 00189 }; 00190 00191 #endif