EmergencyStopper.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef EMERGENCY_STOPPER_H
00011 #define EMERGENCY_STOPPER_H
00012 
00013 #include <rtm/idl/BasicDataType.hh>
00014 #include <rtm/idl/ExtendedDataTypes.hh>
00015 #include "hrpsys/idl/HRPDataTypes.hh"
00016 #include <rtm/Manager.h>
00017 #include <rtm/DataFlowComponentBase.h>
00018 #include <rtm/CorbaPort.h>
00019 #include <rtm/DataInPort.h>
00020 #include <rtm/DataOutPort.h>
00021 #include <rtm/idl/BasicDataTypeSkel.h>
00022 #include <rtm/idl/ExtendedDataTypesSkel.h>
00023 #include <hrpModel/Body.h>
00024 #include "interpolator.h"
00025 #include <queue>
00026 
00027 // Service implementation headers
00028 // <rtc-template block="service_impl_h">
00029 #include "EmergencyStopperService_impl.h"
00030 #include "../SoftErrorLimiter/beep.h"
00031 
00032 // </rtc-template>
00033 
00034 // Service Consumer stub headers
00035 // <rtc-template block="consumer_stub_h">
00036 
00037 // </rtc-template>
00038 
00039 using namespace RTC;
00040 
00044 class EmergencyStopper
00045     : public RTC::DataFlowComponentBase
00046 {
00047 public:
00052     EmergencyStopper(RTC::Manager* manager);
00056     virtual ~EmergencyStopper();
00057 
00058     // The initialize action (on CREATED->ALIVE transition)
00059     // formaer rtc_init_entry()
00060     virtual RTC::ReturnCode_t onInitialize();
00061 
00062     // The finalize action (on ALIVE->END transition)
00063     // formaer rtc_exiting_entry()
00064     virtual RTC::ReturnCode_t onFinalize();
00065 
00066     // The startup action when ExecutionContext startup
00067     // former rtc_starting_entry()
00068     // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00069 
00070     // The shutdown action when ExecutionContext stop
00071     // former rtc_stopping_entry()
00072     // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00073 
00074     // The activated action (Active state entry action)
00075     // former rtc_active_entry()
00076     virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00077 
00078     // The deactivated action (Active state exit action)
00079     // former rtc_active_exit()
00080     virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00081 
00082     // The execution action that is invoked periodically
00083     // former rtc_active_do()
00084     virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00085 
00086     // The aborting action when main logic error occurred.
00087     // former rtc_aborting_entry()
00088     // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00089 
00090     // The error action in ERROR state
00091     // former rtc_error_do()
00092     // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00093 
00094     // The reset action that is invoked resetting
00095     // This is same but different the former rtc_init_entry()
00096     // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00097 
00098     // The state update action that is invoked after onExecute() action
00099     // no corresponding operation exists in OpenRTm-aist-0.2.0
00100     // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00101 
00102     // The action that is invoked when execution context's rate is changed
00103     // no corresponding operation exists in OpenRTm-aist-0.2.0
00104     // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00105     bool stopMotion();
00106     bool releaseMotion();
00107     bool getEmergencyStopperParam(OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param);
00108     bool setEmergencyStopperParam(const OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param);
00109 
00110 protected:
00111     // Configuration variable declaration
00112     // <rtc-template block="config_declare">
00113   
00114     // </rtc-template>
00115 
00116     TimedDoubleSeq m_qRef;
00117     TimedDoubleSeq m_q;
00118     TimedLong m_emergencySignal;
00119     TimedLong m_emergencyMode;
00120     OpenHRP::TimedLongSeqSeq m_servoState;
00121     std::vector<TimedDoubleSeq> m_wrenchesRef;
00122     std::vector<TimedDoubleSeq> m_wrenches;
00123     TimedLongSeq m_beepCommand;
00124 
00125     // DataInPort declaration
00126     // <rtc-template block="inport_declare">
00127     InPort<TimedDoubleSeq> m_qRefIn;
00128     InPort<TimedLong> m_emergencySignalIn;
00129     InPort<OpenHRP::TimedLongSeqSeq> m_servoStateIn;
00130     std::vector<InPort<TimedDoubleSeq> *> m_wrenchesIn;
00131   
00132     // </rtc-template>
00133 
00134     // DataOutPort declaration
00135     // <rtc-template block="outport_declare">
00136     OutPort<TimedDoubleSeq> m_qOut;
00137     OutPort<TimedLong> m_emergencyModeOut;
00138     std::vector<OutPort<TimedDoubleSeq> *> m_wrenchesOut;
00139     OutPort<TimedLongSeq> m_beepCommandOut;
00140   
00141     // </rtc-template>
00142 
00143     // CORBA Port declaration
00144     // <rtc-template block="corbaport_declare">
00145   
00146     // </rtc-template>
00147 
00148     // Service declaration
00149     // <rtc-template block="service_declare">
00150     RTC::CorbaPort m_EmergencyStopperServicePort;
00151   
00152     // </rtc-template>
00153 
00154     // Consumer declaration
00155     // <rtc-template block="consumer_declare">
00156     EmergencyStopperService_impl m_service0;
00157   
00158     // </rtc-template>
00159 
00160 private:
00161     void get_wrenches_array_from_data(const std::vector<TimedDoubleSeq> &wrenches_data, double *wrenches_array) {
00162         for ( unsigned int i= 0; i < wrenches_data.size(); i++ ) {
00163             for (int j = 0; j < 6; j++ ) {
00164                 wrenches_array[i*6+j] = wrenches_data[i].data[j];
00165             }
00166         }
00167     }
00168 
00169     void set_wrenches_data_from_array(std::vector<TimedDoubleSeq> &wrenches_data, const double *wrenches_array) {
00170         for ( unsigned int i= 0; i < wrenches_data.size(); i++ ) {
00171             for (int j = 0; j < 6; j++ ) {
00172                 wrenches_data[i].data[j] = wrenches_array[i*6+j];
00173             }
00174         }
00175     }
00176 
00177     hrp::BodyPtr m_robot;
00178     double m_dt;
00179     unsigned int m_debugLevel;
00180     int loop;
00181     bool is_stop_mode, prev_is_stop_mode;
00182     bool is_initialized;
00183     int recover_time, retrieve_time;
00184     double recover_time_dt;
00185     int default_recover_time, default_retrieve_time;
00186     double *m_stop_posture;
00187     double *m_stop_wrenches;
00188     double *m_tmp_wrenches;
00189     interpolator* m_interpolator;
00190     interpolator* m_wrenches_interpolator;
00191     std::queue<std::vector<double> > m_input_posture_queue;
00192     std::queue<std::vector<double> > m_input_wrenches_queue;
00193     int emergency_stopper_beep_count, emergency_stopper_beep_freq;
00194     coil::Mutex m_mutex;
00195     BeepClient bc;
00196     int dummy;
00197 };
00198 
00199 
00200 extern "C"
00201 {
00202     void EmergencyStopperInit(RTC::Manager* manager);
00203 };
00204 
00205 #endif // EMERGENCY_STOPPER_H


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