EmergencyStopper.h
Go to the documentation of this file.
1 // -*- C++ -*-
10 #ifndef EMERGENCY_STOPPER_H
11 #define EMERGENCY_STOPPER_H
12 
13 #include <rtm/idl/BasicDataType.hh>
14 #include <rtm/idl/ExtendedDataTypes.hh>
15 #include "hrpsys/idl/HRPDataTypes.hh"
16 #include <rtm/Manager.h>
17 #include <rtm/DataFlowComponentBase.h>
18 #include <rtm/CorbaPort.h>
19 #include <rtm/DataInPort.h>
20 #include <rtm/DataOutPort.h>
21 #include <rtm/idl/BasicDataTypeSkel.h>
22 #include <rtm/idl/ExtendedDataTypesSkel.h>
23 #include <hrpModel/Body.h>
24 #include "interpolator.h"
25 #include <queue>
26 
27 // Service implementation headers
28 // <rtc-template block="service_impl_h">
30 #include "../SoftErrorLimiter/beep.h"
31 
32 // </rtc-template>
33 
34 // Service Consumer stub headers
35 // <rtc-template block="consumer_stub_h">
36 
37 // </rtc-template>
38 
39 using namespace RTC;
40 
46 {
47 public:
56  virtual ~EmergencyStopper();
57 
58  // The initialize action (on CREATED->ALIVE transition)
59  // formaer rtc_init_entry()
60  virtual RTC::ReturnCode_t onInitialize();
61 
62  // The finalize action (on ALIVE->END transition)
63  // formaer rtc_exiting_entry()
64  virtual RTC::ReturnCode_t onFinalize();
65 
66  // The startup action when ExecutionContext startup
67  // former rtc_starting_entry()
68  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
69 
70  // The shutdown action when ExecutionContext stop
71  // former rtc_stopping_entry()
72  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
73 
74  // The activated action (Active state entry action)
75  // former rtc_active_entry()
76  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
77 
78  // The deactivated action (Active state exit action)
79  // former rtc_active_exit()
80  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
81 
82  // The execution action that is invoked periodically
83  // former rtc_active_do()
84  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
85 
86  // The aborting action when main logic error occurred.
87  // former rtc_aborting_entry()
88  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
89 
90  // The error action in ERROR state
91  // former rtc_error_do()
92  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
93 
94  // The reset action that is invoked resetting
95  // This is same but different the former rtc_init_entry()
96  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
97 
98  // The state update action that is invoked after onExecute() action
99  // no corresponding operation exists in OpenRTm-aist-0.2.0
100  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
101 
102  // The action that is invoked when execution context's rate is changed
103  // no corresponding operation exists in OpenRTm-aist-0.2.0
104  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
105  bool stopMotion();
106  bool releaseMotion();
107  bool getEmergencyStopperParam(OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param);
108  bool setEmergencyStopperParam(const OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param);
109 
110 protected:
111  // Configuration variable declaration
112  // <rtc-template block="config_declare">
113 
114  // </rtc-template>
115 
116  TimedDoubleSeq m_qRef;
117  TimedDoubleSeq m_q;
118  TimedLong m_emergencySignal;
119  TimedLong m_emergencyMode;
120  OpenHRP::TimedLongSeqSeq m_servoState;
121  std::vector<TimedDoubleSeq> m_wrenchesRef;
122  std::vector<TimedDoubleSeq> m_wrenches;
123  TimedLongSeq m_beepCommand;
124 
125  // DataInPort declaration
126  // <rtc-template block="inport_declare">
130  std::vector<InPort<TimedDoubleSeq> *> m_wrenchesIn;
131 
132  // </rtc-template>
133 
134  // DataOutPort declaration
135  // <rtc-template block="outport_declare">
138  std::vector<OutPort<TimedDoubleSeq> *> m_wrenchesOut;
140 
141  // </rtc-template>
142 
143  // CORBA Port declaration
144  // <rtc-template block="corbaport_declare">
145 
146  // </rtc-template>
147 
148  // Service declaration
149  // <rtc-template block="service_declare">
151 
152  // </rtc-template>
153 
154  // Consumer declaration
155  // <rtc-template block="consumer_declare">
157 
158  // </rtc-template>
159 
160 private:
161  void get_wrenches_array_from_data(const std::vector<TimedDoubleSeq> &wrenches_data, double *wrenches_array) {
162  for ( unsigned int i= 0; i < wrenches_data.size(); i++ ) {
163  for (int j = 0; j < 6; j++ ) {
164  wrenches_array[i*6+j] = wrenches_data[i].data[j];
165  }
166  }
167  }
168 
169  void set_wrenches_data_from_array(std::vector<TimedDoubleSeq> &wrenches_data, const double *wrenches_array) {
170  for ( unsigned int i= 0; i < wrenches_data.size(); i++ ) {
171  for (int j = 0; j < 6; j++ ) {
172  wrenches_data[i].data[j] = wrenches_array[i*6+j];
173  }
174  }
175  }
176 
178  double m_dt;
179  unsigned int m_debugLevel;
180  int loop;
181  bool is_stop_mode, prev_is_stop_mode;
183  int recover_time, retrieve_time;
185  int default_recover_time, default_retrieve_time;
186  double *m_stop_posture;
188  double *m_tmp_wrenches;
191  std::queue<std::vector<double> > m_input_posture_queue;
192  std::queue<std::vector<double> > m_input_wrenches_queue;
193  int emergency_stopper_beep_count, emergency_stopper_beep_freq;
196  int dummy;
197 };
198 
199 
200 extern "C"
201 {
203 };
204 
205 #endif // EMERGENCY_STOPPER_H
ec_id
TimedDoubleSeq m_q
interpolator * m_wrenches_interpolator
OutPort< TimedLong > m_emergencyModeOut
OutPort< TimedLongSeq > m_beepCommandOut
TimedDoubleSeq m_qRef
TimedLong m_emergencySignal
TimedLong m_emergencyMode
void get_wrenches_array_from_data(const std::vector< TimedDoubleSeq > &wrenches_data, double *wrenches_array)
manager
png_uint_32 i
TimedLongSeq m_beepCommand
RTC::CorbaPort m_EmergencyStopperServicePort
InPort< TimedDoubleSeq > m_qRefIn
EmergencyStopperService_impl m_service0
std::queue< std::vector< double > > m_input_posture_queue
hrp::BodyPtr m_robot
sample RT component which has one data input port and one data output port
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateIn
ExecutionContextHandle_t UniqueId
interpolator * m_interpolator
std::vector< TimedDoubleSeq > m_wrenchesRef
OpenHRP::TimedLongSeqSeq m_servoState
std::queue< std::vector< double > > m_input_wrenches_queue
std::vector< OutPort< TimedDoubleSeq > * > m_wrenchesOut
unsigned int m_debugLevel
std::vector< InPort< TimedDoubleSeq > * > m_wrenchesIn
InPort< TimedLong > m_emergencySignalIn
void set_wrenches_data_from_array(std::vector< TimedDoubleSeq > &wrenches_data, const double *wrenches_array)
void EmergencyStopperInit(RTC::Manager *manager)
OutPort< TimedDoubleSeq > m_qOut
std::vector< TimedDoubleSeq > m_wrenches


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20