00001
00025 #include "denso_robot_core/denso_variable.h"
00026
00027 #define NAME_READ "_Read"
00028 #define NAME_WRITE "_Write"
00029 #define NAME_ID "_ID"
00030
00031 namespace denso_robot_core {
00032
00033 DensoVariable::DensoVariable(DensoBase* parent,
00034 Service_Vec& service, Handle_Vec& handle,
00035 const std::string& name, const int* mode,
00036 int16_t vt, bool Read, bool Write, bool ID, int Duration)
00037 : DensoBase(parent, service, handle, name, mode),
00038 m_vt(vt), m_bRead(Read), m_bWrite(Write), m_bID(ID)
00039 {
00040 m_Duration = ros::Duration(Duration / 1000, (Duration % 1000) * 1000);
00041 m_pubTimePrev = ros::Time::now();
00042 }
00043
00044 DensoVariable::~DensoVariable()
00045 {
00046
00047 }
00048
00049 HRESULT DensoVariable::StartService(ros::NodeHandle& node)
00050 {
00051 if(*m_mode != 0) {
00052 return S_FALSE;
00053 }
00054
00055
00056 std::string tmpName = m_parent->RosName();
00057 if(tmpName != "") tmpName.append("/");
00058 tmpName.append(DensoBase::RosName());
00059
00060 if(m_bRead) {
00061 switch(m_vt) {
00062 case VT_I4:
00063 m_pubValue = node.advertise<Int32>(
00064 tmpName + NAME_READ, MESSAGE_QUEUE);
00065 break;
00066 case VT_R4:
00067 m_pubValue = node.advertise<Float32>(
00068 tmpName + NAME_READ, MESSAGE_QUEUE);
00069 break;
00070 case VT_R8:
00071 m_pubValue = node.advertise<Float64>(
00072 tmpName + NAME_READ, MESSAGE_QUEUE);
00073 break;
00074 case VT_BSTR:
00075 m_pubValue = node.advertise<String>(
00076 tmpName + NAME_READ, MESSAGE_QUEUE);
00077 break;
00078 case VT_BOOL:
00079 m_pubValue = node.advertise<Bool>(
00080 tmpName + NAME_READ, MESSAGE_QUEUE);
00081 break;
00082 case (VT_ARRAY | VT_R4):
00083 m_pubValue = node.advertise<Float32MultiArray>(
00084 tmpName + NAME_READ, MESSAGE_QUEUE);
00085 break;
00086 case (VT_ARRAY | VT_R8):
00087 m_pubValue = node.advertise<Float64MultiArray>(
00088 tmpName + NAME_READ, MESSAGE_QUEUE);
00089 break;
00090 default:
00091 return E_FAIL;
00092 }
00093 }
00094
00095 if(m_bWrite) {
00096 switch(m_vt) {
00097 case VT_I4:
00098 m_subValue = node.subscribe<Int32>(
00099 tmpName + NAME_WRITE, MESSAGE_QUEUE,
00100 &DensoVariable::Callback_I32, this);
00101 break;
00102 case VT_R4:
00103 m_subValue = node.subscribe<Float32>(
00104 tmpName + NAME_WRITE, MESSAGE_QUEUE,
00105 &DensoVariable::Callback_F32, this);
00106 break;
00107 case VT_R8:
00108 m_subValue = node.subscribe<Float64>(
00109 tmpName + NAME_WRITE, MESSAGE_QUEUE,
00110 &DensoVariable::Callback_F64, this);
00111 break;
00112 case VT_BSTR:
00113 m_subValue = node.subscribe<String>(
00114 tmpName + NAME_WRITE, MESSAGE_QUEUE,
00115 &DensoVariable::Callback_String, this);
00116 break;
00117 case VT_BOOL:
00118 m_subValue = node.subscribe<Bool>(
00119 tmpName + NAME_WRITE, MESSAGE_QUEUE,
00120 &DensoVariable::Callback_Bool, this);
00121 break;
00122 case (VT_ARRAY | VT_R4):
00123 m_subValue = node.subscribe<Float32MultiArray>(
00124 tmpName + NAME_WRITE, MESSAGE_QUEUE,
00125 &DensoVariable::Callback_F32Array, this);
00126 break;
00127 case (VT_ARRAY | VT_R8):
00128 m_subValue = node.subscribe<Float64MultiArray>(
00129 tmpName + NAME_WRITE, MESSAGE_QUEUE,
00130 &DensoVariable::Callback_F64Array, this);
00131 break;
00132 default:
00133 return E_FAIL;
00134 }
00135 }
00136
00137 if(m_bID) {
00138 m_subID = node.subscribe<Int32>(
00139 tmpName + NAME_ID, MESSAGE_QUEUE,
00140 &DensoVariable::Callback_ID, this);
00141 }
00142
00143 m_serving = true;
00144
00145 return S_OK;
00146 }
00147
00148 HRESULT DensoVariable::StopService()
00149 {
00150 m_mtxSrv.lock();
00151 m_serving = false;
00152 m_mtxSrv.unlock();
00153
00154 m_pubValue.shutdown();
00155 m_subValue.shutdown();
00156 m_subID.shutdown();
00157 return S_OK;
00158 }
00159
00160 bool DensoVariable::Update()
00161 {
00162 boost::mutex::scoped_lock lockSrv(m_mtxSrv);
00163 if(!m_serving) return false;
00164
00165 if(m_bRead) {
00166 HRESULT hr;
00167
00168 Int32 varI; Float32 varF; Float64 varD;
00169 String varS; Bool varIO;
00170 Float32MultiArray varFArray;
00171 Float64MultiArray varDArray;
00172
00173 uint32_t num;
00174 float *pfltval;
00175 double *pdblval;
00176
00177 ros::Time pubTimeCur = ros::Time::now();
00178
00179 if(pubTimeCur - m_pubTimePrev > m_Duration) {
00180 VARIANT_Ptr vntRet(new VARIANT());
00181 VariantInit(vntRet.get());
00182
00183 hr = ExecGetValue(vntRet);
00184 if(SUCCEEDED(hr)) {
00185 if(vntRet->vt == m_vt) {
00186 switch(m_vt){
00187 case VT_I4:
00188 varI.data = vntRet->lVal;
00189 m_pubValue.publish(varI);
00190 break;
00191 case VT_R4:
00192 varF.data = vntRet->fltVal;
00193 m_pubValue.publish(varF);
00194 break;
00195 case VT_R8:
00196 varD.data = vntRet->dblVal;
00197 m_pubValue.publish(varD);
00198 break;
00199 case VT_BSTR:
00200 varS.data = ConvertBSTRToString(vntRet->bstrVal);
00201 m_pubValue.publish(varS);
00202 break;
00203 case VT_BOOL:
00204 varIO.data = (vntRet->boolVal != VARIANT_FALSE) ? true : false;
00205 m_pubValue.publish(varIO);
00206 break;
00207 case (VT_ARRAY | VT_R4):
00208 num = vntRet->parray->rgsabound->cElements;
00209 SafeArrayAccessData(vntRet->parray, (void**)&pfltval);
00210 varFArray.data.resize(num);
00211 std::copy(pfltval, &pfltval[num], varFArray.data.begin());
00212 SafeArrayUnaccessData(vntRet->parray);
00213 m_pubValue.publish(varFArray);
00214 break;
00215 case (VT_ARRAY | VT_R8):
00216 num = vntRet->parray->rgsabound->cElements;
00217 SafeArrayAccessData(vntRet->parray, (void**)&pdblval);
00218 varDArray.data.resize(num);
00219 std::copy(pdblval, &pdblval[num], varDArray.data.begin());
00220 SafeArrayUnaccessData(vntRet->parray);
00221 m_pubValue.publish(varDArray);
00222 break;
00223 }
00224 }
00225 }
00226
00227 m_pubTimePrev = pubTimeCur;
00228 }
00229 }
00230
00231 return true;
00232 }
00233
00234 HRESULT DensoVariable::ExecGetValue(VARIANT_Ptr& value)
00235 {
00236 VARIANT_Vec vntArgs;
00237 VARIANT_Ptr vntHandle(new VARIANT());
00238
00239 VariantInit(vntHandle.get());
00240
00241 vntHandle->vt = VT_UI4;
00242 vntHandle->ulVal = m_vecHandle[DensoBase::SRV_WATCH];
00243
00244 vntArgs.push_back(*vntHandle.get());
00245
00246 return m_vecService[DensoBase::SRV_WATCH]->ExecFunction(
00247 ID_VARIABLE_GETVALUE, vntArgs, value);
00248 }
00249
00250 HRESULT DensoVariable::ExecPutValue(const VARIANT_Ptr& value)
00251 {
00252 HRESULT hr;
00253 int argc;
00254 VARIANT_Vec vntArgs;
00255 VARIANT_Ptr vntHandle(new VARIANT());
00256 VARIANT_Ptr vntRet(new VARIANT());
00257
00258 VariantInit(vntRet.get());
00259
00260 vntHandle->vt = VT_UI4;
00261 vntHandle->ulVal = m_vecHandle[DensoBase::SRV_WATCH];
00262
00263 vntArgs.push_back(*vntHandle.get());
00264
00265 vntArgs.push_back(*value.get());
00266
00267 hr = m_vecService[DensoBase::SRV_WATCH]->ExecFunction(
00268 ID_VARIABLE_PUTVALUE, vntArgs, vntRet);
00269 if(SUCCEEDED(hr)) {
00270 Update();
00271 }
00272
00273 return hr;
00274 }
00275
00276 HRESULT DensoVariable::ExecPutID(const int id)
00277 {
00278 HRESULT hr;
00279 int argc;
00280 VARIANT_Vec vntArgs;
00281 VARIANT_Ptr vntHandle(new VARIANT());
00282 VARIANT_Ptr vntValue(new VARIANT());
00283 VARIANT_Ptr vntRet(new VARIANT());
00284
00285 VariantInit(vntRet.get());
00286
00287 vntHandle->vt = VT_UI4;
00288 vntHandle->ulVal = m_vecHandle[DensoBase::SRV_WATCH];
00289
00290 vntArgs.push_back(*vntHandle.get());
00291
00292 vntValue->vt = VT_I4;
00293 vntValue->lVal = id;
00294 vntArgs.push_back(*vntValue.get());
00295
00296 hr = m_vecService[DensoBase::SRV_WATCH]->ExecFunction(
00297 ID_VARIABLE_PUTID, vntArgs, vntRet);
00298 if(SUCCEEDED(hr)) {
00299 Update();
00300 }
00301
00302 return hr;
00303 }
00304
00305 void DensoVariable::Callback_I32(const Int32::ConstPtr& msg)
00306 {
00307 VARIANT_Ptr vntVal(new VARIANT());
00308 vntVal->vt = VT_I4;
00309 vntVal->lVal = msg->data;
00310
00311 ExecPutValue(vntVal);
00312 }
00313
00314 void DensoVariable::Callback_F32(const Float32::ConstPtr& msg)
00315 {
00316 VARIANT_Ptr vntVal(new VARIANT());
00317 vntVal->vt = VT_R4;
00318 vntVal->fltVal = msg->data;
00319
00320 ExecPutValue(vntVal);
00321 }
00322
00323 void DensoVariable::Callback_F64(const Float64::ConstPtr& msg)
00324 {
00325 VARIANT_Ptr vntVal(new VARIANT());
00326 vntVal->vt = VT_R8;
00327 vntVal->dblVal = msg->data;
00328
00329 ExecPutValue(vntVal);
00330 }
00331
00332 void DensoVariable::Callback_String(const String::ConstPtr& msg)
00333 {
00334 VARIANT_Ptr vntVal(new VARIANT());
00335 vntVal->vt = VT_BSTR;
00336 vntVal->bstrVal = ConvertStringToBSTR(msg->data);
00337
00338 ExecPutValue(vntVal);
00339 }
00340
00341 void DensoVariable::Callback_Bool(const Bool::ConstPtr& msg)
00342 {
00343 VARIANT_Ptr vntVal(new VARIANT());
00344 vntVal->vt = VT_BOOL;
00345 vntVal->boolVal = (msg->data != 0) ? VARIANT_TRUE : VARIANT_FALSE;
00346
00347 ExecPutValue(vntVal);
00348 }
00349
00350 void DensoVariable::Callback_F32Array(const Float32MultiArray::ConstPtr& msg)
00351 {
00352 VARIANT_Ptr vntVal(new VARIANT());
00353 float *pval;
00354
00355 vntVal->vt = (VT_ARRAY | VT_R4);
00356 vntVal->parray = SafeArrayCreateVector(VT_R4, 0, msg->data.size());
00357
00358 SafeArrayAccessData(vntVal->parray, (void**)&pval);
00359 std::copy(msg->data.begin(), msg->data.end(), pval);
00360 SafeArrayUnaccessData(vntVal->parray);
00361
00362 ExecPutValue(vntVal);
00363 }
00364
00365 void DensoVariable::Callback_F64Array(const Float64MultiArray::ConstPtr& msg)
00366 {
00367 VARIANT_Ptr vntVal(new VARIANT());
00368 double *pval;
00369
00370 vntVal->vt = (VT_ARRAY | VT_R8);
00371 vntVal->parray = SafeArrayCreateVector(VT_R8, 0, msg->data.size());
00372
00373 SafeArrayAccessData(vntVal->parray, (void**)&pval);
00374 std::copy(msg->data.begin(), msg->data.end(), pval);
00375 SafeArrayUnaccessData(vntVal->parray);
00376
00377 ExecPutValue(vntVal);
00378 }
00379
00380 void DensoVariable::Callback_ID(const Int32::ConstPtr &msg)
00381 {
00382 ExecPutID(msg->data);
00383 }
00384
00385 }