Beeper.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "Beeper.h"
00011 #include <rtm/CorbaNaming.h>
00012 #include<deque>
00013 
00014 // Variables for beep_thread
00015 struct BeepData
00016 {
00017   bool _beep_start;
00018   int _beep_freq;
00019   int _beep_length;
00020 };
00021 
00022 bool is_initialized = false;
00023 pthread_mutex_t beep_mutex;  // Mutex
00024 double m_dt = 0.002;
00025 std::deque<BeepData> beep_command_buffer; // Used for communication between beepthread and real-time thread
00026 
00027 // Module specification
00028 // <rtc-template block="module_spec">
00029 static const char* beeper_spec[] =
00030   {
00031     "implementation_id", "Beeper",
00032     "type_name",         "Beeper",
00033     "description",       "beeper",
00034     "version",           HRPSYS_PACKAGE_VERSION,
00035     "vendor",            "AIST",
00036     "category",          "example",
00037     "activity_type",     "DataFlowComponent",
00038     "max_instance",      "10",
00039     "language",          "C++",
00040     "lang_type",         "compile",
00041     // Configuration variables
00042     "conf.default.debugLevel", "0",
00043     ""
00044   };
00045 // </rtc-template>
00046 
00047 void* call_beep (void* args)
00048 {
00049   // Initialize
00050   init_beep();
00051   bool wait_for_initialized = true;
00052   while (wait_for_initialized) { // Wait until m_dt is set
00053     usleep(2000);
00054     pthread_mutex_lock( &beep_mutex );
00055     wait_for_initialized = !is_initialized;
00056     pthread_mutex_unlock( &beep_mutex );
00057   }
00058   // Loop
00059   bool prev_beep_start=false;
00060   int prev_beep_length = 1000;
00061   while (1) {
00062     // Get beepCommand from buffer
00063     pthread_mutex_lock( &beep_mutex );
00064     BeepData bd = beep_command_buffer.front();
00065     if (beep_command_buffer.size() > 1) beep_command_buffer.pop_front();
00066     pthread_mutex_unlock( &beep_mutex );
00067 //     if (!prev_beep_start && tmp_beep_start) {
00068 //       std::cerr << "BP START" << std::endl;
00069 //     } else if (prev_beep_start && !tmp_beep_start) {
00070 //       std::cerr << "BP STOP" << std::endl;
00071 //     }
00072     // Beep
00073     if (bd._beep_start) {
00074       usleep(std::max(static_cast<int>(prev_beep_length*1000),0));
00075       start_beep(bd._beep_freq, bd._beep_length);
00076     } else if (prev_beep_start) { // If !beep_start and prev_beep_start, stop_beep just once.
00077       usleep(static_cast<size_t>(1000000*m_dt));
00078       stop_beep();
00079     } else {
00080       usleep(static_cast<size_t>(1000000*m_dt));
00081     }
00082     prev_beep_start = bd._beep_start;
00083     prev_beep_length = bd._beep_length;
00084   }
00085   quit_beep();
00086 }
00087 
00088 Beeper::Beeper(RTC::Manager* manager)
00089   : RTC::DataFlowComponentBase(manager),
00090     // <rtc-template block="initializer">
00091     m_beepCommandIn("beepCommand", m_beepCommand),
00092     m_loop(0),
00093     m_debugLevel(0)
00094 {
00095   pthread_create(&beep_thread, NULL, call_beep, (void*)NULL);
00096   pthread_mutex_init( &beep_mutex, NULL );
00097 }
00098 
00099 Beeper::~Beeper()
00100 {
00101   pthread_join(beep_thread, NULL );
00102 }
00103 
00104 RTC::ReturnCode_t Beeper::onInitialize()
00105 {
00106   std::cerr << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl;
00107   // <rtc-template block="bind_config">
00108   // Bind variables and configuration variable
00109   bindParameter("debugLevel", m_debugLevel, "0");
00110   
00111   // </rtc-template>
00112 
00113   // Registration: InPort/OutPort/Service
00114   // <rtc-template block="registration">
00115   // Set InPort buffers
00116   addInPort("beepCommand", m_beepCommandIn);
00117 
00118   // Set OutPort buffer
00119 
00120   // Set service consumers to Ports
00121 
00122   // Set CORBA Service Ports
00123 
00124   // </rtc-template>
00125 
00126   RTC::Properties& prop =  getProperties();
00127   coil::stringTo(m_dt, prop["dt"].c_str());
00128   pthread_mutex_lock( &beep_mutex );
00129   is_initialized = true;
00130   pthread_mutex_unlock( &beep_mutex );
00131   std::cerr << "[" << m_profile.instance_name << "] : Beep thread dt = " << m_dt << "[s]" << std::endl;
00132 
00133   return RTC::RTC_OK;
00134 }
00135 
00136 /*
00137 RTC::ReturnCode_t Beeper::onFinalize()
00138 {
00139   return RTC::RTC_OK;
00140 }
00141 */
00142 
00143 /*
00144 RTC::ReturnCode_t Beeper::onStartup(RTC::UniqueId ec_id)
00145 {
00146   return RTC::RTC_OK;
00147 }
00148 */
00149 
00150 /*
00151 RTC::ReturnCode_t Beeper::onShutdown(RTC::UniqueId ec_id)
00152 {
00153   return RTC::RTC_OK;
00154 }
00155 */
00156 
00157 RTC::ReturnCode_t Beeper::onActivated(RTC::UniqueId ec_id)
00158 {
00159   std::cerr << "[" << m_profile.instance_name << "] : onActivated(" << ec_id << ")" << std::endl;
00160   return RTC::RTC_OK;
00161 }
00162 
00163 RTC::ReturnCode_t Beeper::onDeactivated(RTC::UniqueId ec_id)
00164 {
00165   std::cerr << "[" << m_profile.instance_name << "] : onDeactivated(" << ec_id << ")" << std::endl;
00166   return RTC::RTC_OK;
00167 }
00168 
00169 #define DEBUGP ((m_debugLevel==1 && m_loop%200==0) || m_debugLevel > 1 )
00170 RTC::ReturnCode_t Beeper::onExecute(RTC::UniqueId ec_id)
00171 {
00172   // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << "), data = " << m_data.data << std::endl;
00173   m_loop++;
00174 
00175   if (m_beepCommandIn.isNew()) {
00176     // Read beepCommand from data port
00177     m_beepCommandIn.read();
00178     BeepData bd;
00179     bd._beep_start = (m_beepCommand.data[BEEP_INFO_START] == 1);
00180     bd._beep_freq = m_beepCommand.data[BEEP_INFO_FREQ];
00181     bd._beep_length = m_beepCommand.data[BEEP_INFO_LENGTH];
00182     // Push beepCommand to buffer
00183     size_t max_buffer_length = 10;
00184     if (pthread_mutex_trylock( &beep_mutex ) == 0) {
00185         beep_command_buffer.push_back(bd);
00186         while (beep_command_buffer.size() > max_buffer_length) beep_command_buffer.pop_front();
00187         pthread_mutex_unlock( &beep_mutex );
00188     } else {
00189         std::cerr << "[" << m_profile.instance_name<< "] Mutex trylock failed (loop=" << m_loop << ")" << std::endl;
00190     }
00191     // print
00192     if (m_debugLevel > 0) {
00193       if (bd._beep_start)
00194         std::cerr << "[" << m_profile.instance_name<< "] isNew : beep start (freq=" << bd._beep_freq << ", length=" << bd._beep_length << ", loop=" << m_loop << ")" << std::endl;
00195       else
00196         std::cerr << "[" << m_profile.instance_name<< "] isNew : beep stop (loop=" << m_loop << ")" << std::endl;
00197     }
00198   }
00199 //   if (beep_start) {
00200 //     start_beep(beep_freq, beep_length, true);
00201 //   } else {
00202 //     stop_beep(true);
00203 //   }
00204 
00205   return RTC::RTC_OK;
00206 }
00207 
00208 /*
00209 RTC::ReturnCode_t Beeper::onAborting(RTC::UniqueId ec_id)
00210 {
00211   return RTC::RTC_OK;
00212 }
00213 */
00214 
00215 /*
00216 RTC::ReturnCode_t Beeper::onError(RTC::UniqueId ec_id)
00217 {
00218   return RTC::RTC_OK;
00219 }
00220 */
00221 
00222 /*
00223 RTC::ReturnCode_t Beeper::onReset(RTC::UniqueId ec_id)
00224 {
00225   return RTC::RTC_OK;
00226 }
00227 */
00228 
00229 /*
00230 RTC::ReturnCode_t Beeper::onStateUpdate(RTC::UniqueId ec_id)
00231 {
00232   return RTC::RTC_OK;
00233 }
00234 */
00235 
00236 /*
00237 RTC::ReturnCode_t Beeper::onRateChanged(RTC::UniqueId ec_id)
00238 {
00239   return RTC::RTC_OK;
00240 }
00241 */
00242 
00243 
00244 extern "C"
00245 {
00246 
00247   void BeeperInit(RTC::Manager* manager)
00248   {
00249     RTC::Properties profile(beeper_spec);
00250     manager->registerFactory(profile,
00251                              RTC::Create<Beeper>,
00252                              RTC::Delete<Beeper>);
00253   }
00254 
00255 };
00256 
00257 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:54