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