Fawkes API  Fawkes Development Version
act_thread.h
00001 
00002 /***************************************************************************
00003  *  act_thread.h - Katana plugin act thread
00004  *
00005  *  Created: Mon Jun 08 17:59:57 2009
00006  *  Copyright  2006-2009  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_KATANA_ACT_THREAD_H_
00024 #define __PLUGINS_KATANA_ACT_THREAD_H_
00025 
00026 #include "motion_thread.h"
00027 #include "calib_thread.h"
00028 #include "goto_thread.h"
00029 #include "goto_openrave_thread.h"
00030 #include "gripper_thread.h"
00031 #include "sensacq_thread.h"
00032 #include "motor_control_thread.h"
00033 
00034 #include <core/threading/thread.h>
00035 #include <aspect/clock.h>
00036 #include <aspect/blocked_timing.h>
00037 #include <aspect/logging.h>
00038 #include <aspect/configurable.h>
00039 #include <aspect/blackboard.h>
00040 #include <aspect/tf.h>
00041 #ifdef HAVE_OPENRAVE
00042 #  include <plugins/openrave/aspect/openrave.h>
00043 #endif
00044 #include <blackboard/interface_listener.h>
00045 #include <core/utils/refptr.h>
00046 #ifdef USE_TIMETRACKER
00047 #  include <utils/time/tracker.h>
00048 #endif
00049 #include <string>
00050 #include <memory>
00051 
00052 namespace fawkes {
00053   class KatanaInterface;
00054   class Time;
00055   class KatanaController;
00056 }
00057 
00058 class KatanaActThread
00059 : public fawkes::Thread,
00060   public fawkes::ClockAspect,
00061   public fawkes::BlockedTimingAspect,
00062   public fawkes::LoggingAspect,
00063   public fawkes::ConfigurableAspect,
00064   public fawkes::BlackBoardAspect,
00065   public fawkes::TransformAspect,
00066 #ifdef HAVE_OPENRAVE
00067   public fawkes::OpenRaveAspect,
00068 #endif
00069   public fawkes::BlackBoardInterfaceListener
00070 {
00071  public:
00072   KatanaActThread();
00073   ~KatanaActThread();
00074 
00075   virtual void init();
00076   virtual void finalize();
00077   virtual void once();
00078   virtual void loop();
00079 
00080   // For BlackBoardInterfaceListener
00081   virtual bool bb_interface_message_received(fawkes::Interface *interface,
00082                                              fawkes::Message *message) throw();
00083 
00084   void update_sensor_values();
00085 
00086  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
00087  protected: virtual void run() { Thread::run(); }
00088 
00089  private:
00090   void stop_motion();
00091   void update_position(bool refresh);
00092   void update_sensors(bool refresh);
00093   void update_motors(bool refresh);
00094   void start_motion(fawkes::RefPtr<KatanaMotionThread> motion_thread,
00095                     unsigned int msgid, const char *logmsg, ...);
00096 
00097  private:
00098   fawkes::KatanaInterface *__katana_if;
00099 
00100   std::string    __cfg_controller;
00101   std::string    __cfg_device;
00102   std::string    __cfg_kni_conffile;
00103   bool           __cfg_auto_calibrate;
00104   unsigned int   __cfg_defmax_speed;
00105   unsigned int   __cfg_read_timeout;
00106   unsigned int   __cfg_write_timeout;
00107   unsigned int   __cfg_gripper_pollint;
00108   unsigned int   __cfg_goto_pollint;
00109   float          __cfg_park_x;
00110   float          __cfg_park_y;
00111   float          __cfg_park_z;
00112   float          __cfg_park_phi;
00113   float          __cfg_park_theta;
00114   float          __cfg_park_psi;
00115 
00116   float          __cfg_distance_scale;
00117 
00118   float          __cfg_update_interval;
00119 
00120   std::string    __cfg_frame_kni;
00121   std::string    __cfg_frame_openrave;
00122 
00123   bool           __cfg_OR_enabled;
00124   bool           __cfg_OR_use_viewer;
00125   bool           __cfg_OR_auto_load_ik;
00126   std::string    __cfg_OR_robot_file;
00127   std::string    __cfg_OR_arm_model;
00128 
00129 
00130   std::auto_ptr<KatanaSensorAcquisitionThread> __sensacq_thread;
00131   fawkes::RefPtr<KatanaMotionThread>           __actmot_thread;
00132   fawkes::RefPtr<KatanaCalibrationThread>      __calib_thread;
00133   fawkes::RefPtr<KatanaGotoThread>             __goto_thread;
00134   fawkes::RefPtr<KatanaGripperThread>          __gripper_thread;
00135   fawkes::RefPtr<KatanaMotorControlThread>     __motor_control_thread;
00136 #ifdef HAVE_OPENRAVE
00137   fawkes::RefPtr<KatanaGotoOpenRaveThread>     __goto_openrave_thread;
00138 #endif
00139 
00140   fawkes::RefPtr<fawkes::KatanaController>      __katana;
00141 
00142   fawkes::Time                  *__last_update;
00143 
00144 #ifdef USE_TIMETRACKER
00145   std::auto_ptr<fawkes::TimeTracker> __tt;
00146   unsigned int __tt_count;
00147   unsigned int __ttc_read_sensor;
00148 #endif
00149 };
00150 
00151 
00152 #endif