Fawkes API  Fawkes Development Version
rx28_thread.h
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