Fawkes API  Fawkes Development Version
dcm_thread.h
00001 
00002 /***************************************************************************
00003  *  dcm_thread.h - Provide NaoQi DCM to Fawkes
00004  *
00005  *  Created: Tue May 31 14:59:30 2011
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_NAO_DCM_THREAD_H_
00024 #define __PLUGINS_NAO_DCM_THREAD_H_
00025 
00026 #include <core/threading/thread.h>
00027 #include <aspect/clock.h>
00028 #include <aspect/logging.h>
00029 #include <aspect/configurable.h>
00030 #include <aspect/blackboard.h>
00031 #include <aspect/blocked_timing.h>
00032 #include <plugins/nao/aspect/naoqi.h>
00033 
00034 #include <core/utils/lock_vector.h>
00035 
00036 #include <interfaces/NaoJointPositionInterface.h>
00037 #include <interfaces/NaoSensorInterface.h>
00038 
00039 #include <althread/alprocesssignals.h>
00040 #include <alcommon/alproxy.h>
00041 
00042 #include <vector>
00043 
00044 namespace AL {
00045   class ALMemoryFastAccess;
00046 }
00047 namespace fawkes {
00048   class NaoJointStiffnessInterface;
00049 }
00050 
00051 class NaoQiDCMThread
00052 : public fawkes::Thread,
00053   public fawkes::LoggingAspect,
00054   public fawkes::ConfigurableAspect,
00055   public fawkes::BlockedTimingAspect,
00056   public fawkes::ClockAspect,
00057   public fawkes::BlackBoardAspect,
00058   public fawkes::NaoQiAspect
00059 {
00060  public:
00061   NaoQiDCMThread();
00062   virtual ~NaoQiDCMThread();
00063 
00064   virtual void init();
00065   virtual void loop();
00066   virtual void finalize();
00067 
00068  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
00069  protected: virtual void run() { Thread::run(); }
00070 
00071  private:
00072   void dcm_callback();
00073   void read_values();
00074   void update_interfaces(fawkes::NaoJointPositionInterface *joint_pos_if,
00075                          fawkes::NaoJointStiffnessInterface *joint_stiffness_if,
00076                          fawkes::NaoSensorInterface *sensor_if);
00077   void process_messages();
00078   std::vector<std::string> parse_servo_bitfield(unsigned int servos);
00079 
00080 
00081   void send_commands(unsigned int servos, std::string what,
00082                      float value, int time_offset);
00083   void send_command(std::string name, float value,
00084                     std::string kind, int time_offset);
00085 
00086   int ultrasonic_value(fawkes::NaoSensorInterface::UltrasonicDirection direction);
00087 
00088   class HighFreqThread;
00089   HighFreqThread *__highfreq_thread;
00090 
00091  private:
00092   AL::ALPtr<AL::DCMProxy> __dcm;
00093   AL::ALPtr<AL::ALMotionProxy> __almotion;
00094   AL::ALPtr<AL::ALMemoryFastAccess> __memfa;
00095   bool __robocup_version;
00096 
00097   AL::ALProcessSignals::ProcessSignalConnection __dcm_sigconn;
00098 
00099   int                       __dcm_time;
00100   fawkes::LockVector<float> __values;
00101 
00102   fawkes::NaoJointPositionInterface   *__joint_pos_highfreq_if;
00103   fawkes::NaoJointPositionInterface   *__joint_pos_if;
00104   fawkes::NaoJointStiffnessInterface  *__joint_stiffness_highfreq_if;
00105   fawkes::NaoJointStiffnessInterface  *__joint_stiffness_if;
00106   fawkes::NaoSensorInterface          *__sensor_highfreq_if;
00107   fawkes::NaoSensorInterface          *__sensor_if;
00108 
00109   uint8_t                                      __robot_version[4];
00110   fawkes::NaoJointPositionInterface::RobotType __robot_type;
00111   int                                          __usboard_version;
00112 
00113   AL::ALValue __alljoint_names;
00114 };
00115 
00116 #endif