00001
00025 #include "denso_robot_core/denso_robot_rc8.h"
00026
00027 #define BCAP_ROBOT_EXECUTE_ARGS (3)
00028 #define BCAP_ROBOT_HALT_ARGS (2)
00029 #define BCAP_ROBOT_MOVE_ARGS (4)
00030 #define BCAP_ROBOT_SPEED_ARGS (3)
00031 #define BCAP_ROBOT_CHANGE_ARGS (2)
00032
00033 #define NAME_MOVESTRING "_MoveString"
00034 #define NAME_MOVEVALUE "_MoveValue"
00035 #define NAME_DRIVEEXSTRING "_DriveExString"
00036 #define NAME_DRIVEEXVALUE "_DriveExValue"
00037 #define NAME_DRIVEAEXSTRING "_DriveAExString"
00038 #define NAME_DRIVEAEXVALUE "_DriveAExValue"
00039 #define NAME_SPEED "_Speed"
00040 #define NAME_CHANGETOOL "_ChangeTool"
00041 #define NAME_CHANGEWORK "_ChangeWork"
00042
00043 namespace denso_robot_core {
00044
00045 enum {
00046 NUM_POSITION = 7,
00047 NUM_JOINT = 8,
00048 NUM_TRANS = 10
00049 };
00050
00051 enum {
00052 ACT_RESET = -1,
00053 ACT_NONE = 0,
00054 ACT_MOVESTRING,
00055 ACT_MOVEVALUE,
00056 ACT_DRIVEEXSTRING,
00057 ACT_DRIVEEXVALUE,
00058 ACT_DRIVEAEXSTRING,
00059 ACT_DRIVEAEXVALUE,
00060 };
00061
00062 DensoRobotRC8::DensoRobotRC8(DensoBase* parent,
00063 Service_Vec& service, Handle_Vec& handle,
00064 const std::string& name, const int* mode)
00065 : DensoRobot(parent, service, handle, name, mode),
00066 m_curAct(ACT_RESET), m_memTimeout(0), m_memRetry(0),
00067 m_tsfmt(0), m_timestamp(0),
00068 m_sendfmt(0), m_send_miniio(0), m_send_handio(0),
00069 m_recvfmt(0), m_recv_miniio(0), m_recv_handio(0),
00070 m_send_userio_offset(UserIO::MIN_USERIO_OFFSET),
00071 m_send_userio_size(1),
00072 m_recv_userio_offset(UserIO::MIN_USERIO_OFFSET),
00073 m_recv_userio_size(1)
00074 {
00075 m_tsfmt = TSFMT_MILLISEC;
00076
00077 m_sendfmt = SENDFMT_MINIIO | SENDFMT_HANDIO;
00078
00079 m_recvfmt = RECVFMT_POSE_PJ |
00080 RECVFMT_MINIIO | RECVFMT_HANDIO;
00081 }
00082
00083 DensoRobotRC8::~DensoRobotRC8()
00084 {
00085
00086 }
00087
00088 HRESULT DensoRobotRC8::StartService(ros::NodeHandle& node)
00089 {
00090 std::string tmpName = DensoBase::RosName();
00091
00092 if(*m_mode == 0) {
00093 m_subSpeed = node.subscribe<Float32>(
00094 tmpName + NAME_SPEED, MESSAGE_QUEUE,
00095 &DensoRobotRC8::Callback_Speed, this);
00096
00097 m_subChangeTool = node.subscribe<Int32>(
00098 tmpName + NAME_CHANGETOOL, MESSAGE_QUEUE,
00099 boost::bind(&DensoRobotRC8::Callback_Change, this, "Tool", _1));
00100
00101 m_subChangeWork = node.subscribe<Int32>(
00102 tmpName + NAME_CHANGEWORK, MESSAGE_QUEUE,
00103 boost::bind(&DensoRobotRC8::Callback_Change, this, "Work", _1));
00104
00105 m_actMoveString = boost::make_shared<SimpleActionServer<MoveStringAction> >
00106 (node, DensoBase::RosName() + NAME_MOVESTRING,
00107 boost::bind(&DensoRobotRC8::Callback_MoveString, this, _1), false);
00108
00109 m_actMoveString->registerPreemptCallback(
00110 boost::bind(&DensoRobotRC8::Callback_Cancel, this));
00111
00112 m_actMoveString->start();
00113
00114 m_actMoveValue = boost::make_shared<SimpleActionServer<MoveValueAction> >
00115 (node, DensoBase::RosName() + NAME_MOVEVALUE,
00116 boost::bind(&DensoRobotRC8::Callback_MoveValue, this, _1), false);
00117
00118 m_actMoveValue->registerPreemptCallback(
00119 boost::bind(&DensoRobotRC8::Callback_Cancel, this));
00120
00121 m_actMoveValue->start();
00122
00123 m_actDriveExString = boost::make_shared<SimpleActionServer<DriveStringAction> >
00124 (node, DensoBase::RosName() + NAME_DRIVEEXSTRING,
00125 boost::bind(&DensoRobotRC8::Callback_DriveString, this, "DriveEx", _1), false);
00126
00127 m_actDriveExString->registerPreemptCallback(
00128 boost::bind(&DensoRobotRC8::Callback_Cancel, this));
00129
00130 m_actDriveExString->start();
00131
00132 m_actDriveExValue = boost::make_shared<SimpleActionServer<DriveValueAction> >
00133 (node, DensoBase::RosName() + NAME_DRIVEEXVALUE,
00134 boost::bind(&DensoRobotRC8::Callback_DriveValue, this, "DriveEx", _1), false);
00135
00136 m_actDriveExValue->registerPreemptCallback(
00137 boost::bind(&DensoRobotRC8::Callback_Cancel, this));
00138
00139 m_actDriveExValue->start();
00140
00141 m_actDriveAExString = boost::make_shared<SimpleActionServer<DriveStringAction> >
00142 (node, DensoBase::RosName() + NAME_DRIVEAEXSTRING,
00143 boost::bind(&DensoRobotRC8::Callback_DriveString, this, "DriveAEx", _1), false);
00144
00145 m_actDriveAExString->registerPreemptCallback(
00146 boost::bind(&DensoRobotRC8::Callback_Cancel, this));
00147
00148 m_actDriveAExString->start();
00149
00150 m_actDriveAExValue = boost::make_shared<SimpleActionServer<DriveValueAction> >
00151 (node, DensoBase::RosName() + NAME_DRIVEAEXVALUE,
00152 boost::bind(&DensoRobotRC8::Callback_DriveValue, this, "DriveAEx", _1), false);
00153
00154 m_actDriveAExValue->registerPreemptCallback(
00155 boost::bind(&DensoRobotRC8::Callback_Cancel, this));
00156
00157 m_actDriveAExValue->start();
00158 }
00159
00160 DensoRobot::StartService(node);
00161
00162 m_curAct = ACT_NONE;
00163
00164 return S_OK;
00165 }
00166
00167 HRESULT DensoRobotRC8::StopService()
00168 {
00169 DensoRobot::StopService();
00170
00171 m_mtxAct.lock();
00172 m_curAct = ACT_RESET;
00173 m_mtxAct.unlock();
00174
00175 m_subSpeed.shutdown();
00176 m_subChangeTool.shutdown();
00177 m_subChangeWork.shutdown();
00178 m_actMoveString.reset();
00179 m_actMoveValue.reset();
00180 m_actDriveExString.reset();
00181 m_actDriveExValue.reset();
00182 m_actDriveAExString.reset();
00183 m_actDriveAExValue.reset();
00184
00185 return S_OK;
00186 }
00187
00188 bool DensoRobotRC8::Update()
00189 {
00190 if(!DensoRobot::Update()) return false;
00191
00192 boost::mutex::scoped_lock lockSrv(m_mtxSrv);
00193 if(!m_serving) return false;
00194
00195 Action_Feedback();
00196
00197 return true;
00198 }
00199
00200 HRESULT DensoRobotRC8::ExecTakeArm()
00201 {
00202 int argc;
00203 VARIANT_Vec vntArgs;
00204 VARIANT_Ptr vntRet(new VARIANT());
00205 int32_t *pval;
00206
00207 VariantInit(vntRet.get());
00208
00209 for(argc = 0; argc < BCAP_ROBOT_EXECUTE_ARGS; argc++) {
00210 VARIANT_Ptr vntTmp(new VARIANT());
00211 VariantInit(vntTmp.get());
00212
00213 switch(argc) {
00214 case 0:
00215 vntTmp->vt = VT_UI4;
00216 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00217 break;
00218 case 1:
00219 vntTmp->vt = VT_BSTR;
00220 vntTmp->bstrVal = SysAllocString(L"TakeArm");
00221 break;
00222 case 2:
00223 vntTmp->vt = (VT_ARRAY | VT_I4);
00224 vntTmp->parray = SafeArrayCreateVector(VT_I4, 0, 2);
00225 SafeArrayAccessData(vntTmp->parray, (void**)&pval);
00226 pval[0] = m_ArmGroup;
00227 pval[1] = 1L;
00228 SafeArrayUnaccessData(vntTmp->parray);
00229 break;
00230 }
00231
00232 vntArgs.push_back(*vntTmp.get());
00233 }
00234
00235 return m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
00236 }
00237
00238 HRESULT DensoRobotRC8::ExecGiveArm()
00239 {
00240 int argc;
00241 VARIANT_Vec vntArgs;
00242 VARIANT_Ptr vntRet(new VARIANT());
00243
00244 VariantInit(vntRet.get());
00245
00246 for(argc = 0; argc < BCAP_ROBOT_EXECUTE_ARGS; argc++) {
00247 VARIANT_Ptr vntTmp(new VARIANT());
00248 VariantInit(vntTmp.get());
00249
00250 switch(argc) {
00251 case 0:
00252 vntTmp->vt = VT_UI4;
00253 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00254 break;
00255 case 1:
00256 vntTmp->vt = VT_BSTR;
00257 vntTmp->bstrVal = SysAllocString(L"GiveArm");
00258 break;
00259 }
00260
00261 vntArgs.push_back(*vntTmp.get());
00262 }
00263
00264 return m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
00265 }
00266
00267 HRESULT DensoRobotRC8::ExecMove(
00268 int comp, const VARIANT_Ptr& pose, const std::string& option)
00269 {
00270 HRESULT hr;
00271
00272 hr = ExecTakeArm();
00273 if(SUCCEEDED(hr)) {
00274 int argc;
00275 VARIANT_Vec vntArgs;
00276 VARIANT_Ptr vntRet(new VARIANT());
00277
00278 VariantInit(vntRet.get());
00279
00280 for(argc = 0; argc < BCAP_ROBOT_MOVE_ARGS; argc++) {
00281 VARIANT_Ptr vntTmp(new VARIANT());
00282 VariantInit(vntTmp.get());
00283
00284 switch(argc) {
00285 case 0:
00286 vntTmp->vt = VT_UI4;
00287 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00288 break;
00289 case 1:
00290 vntTmp->vt = VT_I4;
00291 vntTmp->lVal = comp;
00292 break;
00293 case 2:
00294 VariantCopy(vntTmp.get(), pose.get());
00295 break;
00296 case 3:
00297 vntTmp->vt = VT_BSTR;
00298 vntTmp->bstrVal = ConvertStringToBSTR(option);
00299 break;
00300 }
00301
00302 vntArgs.push_back(*vntTmp.get());
00303 }
00304
00305 hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_MOVE, vntArgs, vntRet);
00306
00307 ExecGiveArm();
00308 }
00309
00310 return hr;
00311 }
00312
00313 HRESULT DensoRobotRC8::ExecDrive(
00314 const std::string& name, const VARIANT_Ptr& option)
00315 {
00316 HRESULT hr;
00317
00318 hr = ExecTakeArm();
00319 if(SUCCEEDED(hr)) {
00320 int argc;
00321 VARIANT_Vec vntArgs;
00322 VARIANT_Ptr vntRet(new VARIANT());
00323
00324 VariantInit(vntRet.get());
00325
00326 for(int argc = 0; argc < BCAP_ROBOT_EXECUTE_ARGS; argc++) {
00327 VARIANT_Ptr vntTmp(new VARIANT());
00328 VariantInit(vntTmp.get());
00329
00330 switch(argc) {
00331 case 0:
00332 vntTmp->vt = VT_UI4;
00333 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00334 break;
00335 case 1:
00336 vntTmp->vt = VT_BSTR;
00337 vntTmp->bstrVal = ConvertStringToBSTR(name);
00338 break;
00339 case 2:
00340 VariantCopy(vntTmp.get(), option.get());
00341 break;
00342 }
00343
00344 vntArgs.push_back(*vntTmp.get());
00345 }
00346
00347 hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
00348
00349 ExecGiveArm();
00350 }
00351
00352 return hr;
00353 }
00354
00355 HRESULT DensoRobotRC8::ExecSpeed(float value)
00356 {
00357 HRESULT hr;
00358
00359 hr = ExecTakeArm();
00360 if(SUCCEEDED(hr)) {
00361 int argc;
00362 VARIANT_Vec vntArgs;
00363 VARIANT_Ptr vntRet(new VARIANT());
00364
00365 VariantInit(vntRet.get());
00366
00367 for(argc = 0; argc < BCAP_ROBOT_SPEED_ARGS; argc++) {
00368 VARIANT_Ptr vntTmp(new VARIANT());
00369 VariantInit(vntTmp.get());
00370
00371 switch(argc) {
00372 case 0:
00373 vntTmp->vt = VT_UI4;
00374 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00375 break;
00376 case 1:
00377 vntTmp->vt = VT_I4;
00378 vntTmp->lVal = -1;
00379 break;
00380 case 2:
00381 vntTmp->vt = VT_R4;
00382 vntTmp->fltVal = value;
00383 break;
00384 }
00385
00386 vntArgs.push_back(*vntTmp.get());
00387 }
00388
00389 hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_SPEED, vntArgs, vntRet);
00390
00391 ExecGiveArm();
00392 }
00393
00394 return hr;
00395 }
00396
00397 HRESULT DensoRobotRC8::ExecChange(const std::string& value)
00398 {
00399 HRESULT hr;
00400
00401 hr = ExecTakeArm();
00402 if(SUCCEEDED(hr)) {
00403 int argc;
00404 VARIANT_Vec vntArgs;
00405 VARIANT_Ptr vntRet(new VARIANT());
00406
00407 VariantInit(vntRet.get());
00408
00409 for(argc = 0; argc < BCAP_ROBOT_CHANGE_ARGS; argc++) {
00410 VARIANT_Ptr vntTmp(new VARIANT());
00411 VariantInit(vntTmp.get());
00412
00413 switch(argc) {
00414 case 0:
00415 vntTmp->vt = VT_UI4;
00416 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00417 break;
00418 case 1:
00419 vntTmp->vt = VT_BSTR;
00420 vntTmp->bstrVal = ConvertStringToBSTR(value);
00421 break;
00422 }
00423
00424 vntArgs.push_back(*vntTmp.get());
00425 }
00426
00427 hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_CHANGE, vntArgs, vntRet);
00428
00429 ExecGiveArm();
00430 }
00431
00432 return hr;
00433 }
00434
00435 HRESULT DensoRobotRC8::ExecHalt()
00436 {
00437 int argc;
00438 VARIANT_Vec vntArgs;
00439 VARIANT_Ptr vntRet(new VARIANT());
00440
00441 for(argc = 0; argc < BCAP_ROBOT_HALT_ARGS; argc++) {
00442 VARIANT_Ptr vntTmp(new VARIANT());
00443
00444 VariantInit(vntTmp.get());
00445
00446 switch(argc) {
00447 case 0:
00448 vntTmp->vt = VT_UI4;
00449 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_WATCH];
00450 break;
00451 case 1:
00452 vntTmp->vt = VT_BSTR;
00453 vntTmp->bstrVal = SysAllocString(L"");
00454 break;
00455 }
00456
00457 vntArgs.push_back(*vntTmp.get());
00458 }
00459
00460 return m_vecService[DensoBase::SRV_WATCH]->ExecFunction(ID_ROBOT_HALT, vntArgs, vntRet);
00461 }
00462
00463 HRESULT DensoRobotRC8::ExecCurJnt(std::vector<double>& pose)
00464 {
00465 HRESULT hr;
00466
00467 int argc;
00468 VARIANT_Vec vntArgs;
00469 VARIANT_Ptr vntRet(new VARIANT());
00470
00471 VariantInit(vntRet.get());
00472
00473 for(argc = 0; argc < BCAP_ROBOT_EXECUTE_ARGS; argc++) {
00474 VARIANT_Ptr vntTmp(new VARIANT());
00475 VariantInit(vntTmp.get());
00476
00477 switch(argc) {
00478 case 0:
00479 vntTmp->vt = VT_UI4;
00480 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_WATCH];
00481 break;
00482 case 1:
00483 vntTmp->vt = VT_BSTR;
00484 vntTmp->bstrVal = SysAllocString(L"HighCurJntEx");
00485 break;
00486 }
00487
00488 vntArgs.push_back(*vntTmp.get());
00489 }
00490
00491 hr = m_vecService[DensoBase::SRV_WATCH]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
00492
00493 if(SUCCEEDED(hr) &&
00494 (vntRet->vt == (VT_ARRAY | VT_R8)))
00495 {
00496 uint32_t num;
00497 double *pdblval;
00498
00499 num = vntRet->parray->rgsabound->cElements;
00500 SafeArrayAccessData(vntRet->parray, (void**)&pdblval);
00501 pose.resize(num - 1);
00502 std::copy(&pdblval[1], &pdblval[num], pose.begin());
00503 SafeArrayUnaccessData(vntRet->parray);
00504 }
00505
00506 return hr;
00507 }
00508
00509 HRESULT DensoRobotRC8::ExecSlaveMove(
00510 const std::vector<double>& pose, std::vector<double>& joint)
00511 {
00512 HRESULT hr = S_OK;
00513 int argc;
00514 VARIANT_Vec vntArgs;
00515 VARIANT_Ptr vntRet(new VARIANT());
00516
00517 VariantInit(vntRet.get());
00518
00519 for(argc = 0; argc < BCAP_ROBOT_EXECUTE_ARGS; argc++) {
00520 VARIANT_Ptr vntTmp(new VARIANT());
00521 VariantInit(vntTmp.get());
00522
00523 switch(argc) {
00524 case 0:
00525 vntTmp->vt = VT_UI4;
00526 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00527 break;
00528 case 1:
00529 vntTmp->vt = VT_BSTR;
00530 vntTmp->bstrVal = SysAllocString(L"slvMove");
00531 break;
00532 case 2:
00533 hr = CreateSendParameter(pose, vntTmp,
00534 m_send_miniio, m_send_handio,
00535 m_recv_userio_offset, m_recv_userio_size,
00536 m_send_userio_offset, m_send_userio_size,
00537 m_send_userio);
00538 if(FAILED(hr)) return hr;
00539 break;
00540 }
00541
00542 vntArgs.push_back(*vntTmp.get());
00543 }
00544
00545 hr = m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
00546 if(SUCCEEDED(hr)) {
00547 HRESULT hrTmp = ParseRecvParameter(vntRet,
00548 m_position, m_joint, m_trans,
00549 m_recv_miniio, m_recv_handio, m_timestamp,
00550 m_recv_userio, m_current);
00551
00552 joint = m_joint;
00553
00554 if(FAILED(hrTmp)) {
00555 hr = hrTmp;
00556 }
00557 }
00558
00559 return hr;
00560 }
00561
00562 HRESULT DensoRobotRC8::ChangeMode(int mode)
00563 {
00564 HRESULT hr = S_OK;
00565
00566 if(*m_mode == 0) {
00567
00568 if(mode != 0) {
00569 hr = ExecSlaveMode("slvSendFormat", m_sendfmt);
00570 if(FAILED(hr)) return hr;
00571
00572 hr = ExecSlaveMode("slvRecvFormat", m_recvfmt, m_tsfmt);
00573 if(FAILED(hr)) return hr;
00574
00575 hr = ExecTakeArm();
00576 if(FAILED(hr)) return hr;
00577
00578 hr = ExecSlaveMode("slvChangeMode", mode);
00579 if(FAILED(hr)) return hr;
00580
00581 m_memTimeout = m_vecService[DensoBase::SRV_ACT]->get_Timeout();
00582 m_memRetry = m_vecService[DensoBase::SRV_ACT]->get_Retry();
00583 m_vecService[DensoBase::SRV_ACT]->put_Timeout(8);
00584 m_vecService[DensoBase::SRV_ACT]->put_Retry(3);
00585 }
00586 } else {
00587 m_vecService[DensoBase::SRV_ACT]->put_Timeout(m_memTimeout);
00588 m_vecService[DensoBase::SRV_ACT]->put_Retry(m_memRetry);
00589
00590 hr = ExecSlaveMode("slvChangeMode", mode);
00591 ExecGiveArm();
00592 }
00593
00594 return hr;
00595 }
00596
00597 HRESULT DensoRobotRC8::ExecSlaveMode(
00598 const std::string& name, int32_t format, int32_t option)
00599 {
00600 int argc;
00601 VARIANT_Vec vntArgs;
00602 VARIANT_Ptr vntRet(new VARIANT());
00603 int32_t *pval;
00604
00605 VariantInit(vntRet.get());
00606
00607 for(argc = 0; argc < BCAP_ROBOT_EXECUTE_ARGS; argc++) {
00608 VARIANT_Ptr vntTmp(new VARIANT());
00609 VariantInit(vntTmp.get());
00610
00611 switch(argc) {
00612 case 0:
00613 vntTmp->vt = VT_UI4;
00614 vntTmp->ulVal = m_vecHandle[DensoBase::SRV_ACT];
00615 break;
00616 case 1:
00617 vntTmp->vt = VT_BSTR;
00618 vntTmp->bstrVal = ConvertStringToBSTR(name);
00619 break;
00620 case 2:
00621 if(option == 0) {
00622 vntTmp->vt = VT_I4;
00623 vntTmp->lVal = format;
00624 } else {
00625 vntTmp->vt = (VT_ARRAY | VT_I4);
00626 vntTmp->parray = SafeArrayCreateVector(VT_I4, 0, 2);
00627 SafeArrayAccessData(vntTmp->parray, (void**)&pval);
00628 pval[0] = format;
00629 pval[1] = option;
00630 SafeArrayUnaccessData(vntTmp->parray);
00631 }
00632 break;
00633 }
00634
00635 vntArgs.push_back(*vntTmp.get());
00636 }
00637
00638 return m_vecService[DensoBase::SRV_ACT]->ExecFunction(ID_ROBOT_EXECUTE, vntArgs, vntRet);
00639 }
00640
00641 HRESULT DensoRobotRC8::CreateSendParameter(
00642 const std::vector<double>& pose, VARIANT_Ptr& send,
00643 const int miniio, const int handio,
00644 const int recv_userio_offset, const int recv_userio_size,
00645 const int send_userio_offset, const int send_userio_size,
00646 const std::vector<uint8_t>& send_userio)
00647 {
00648 int type = *m_mode & SLVMODE_POSE;
00649
00650
00651 int joints = 0;
00652 switch(type) {
00653 case SLVMODE_POSE_P:
00654 joints = NUM_POSITION;
00655 break;
00656 case SLVMODE_POSE_J:
00657 joints = NUM_JOINT;
00658 break;
00659 case SLVMODE_POSE_T:
00660 joints = NUM_TRANS;
00661 break;
00662 default:
00663 return E_FAIL;
00664 }
00665
00666 if(joints < pose.size()) {
00667 return E_FAIL;
00668 }
00669
00670
00671 bool send_hio, send_mio, send_uio, recv_uio;
00672 send_hio = m_sendfmt & SENDFMT_HANDIO;
00673 send_mio = m_sendfmt & SENDFMT_MINIIO;
00674 send_uio = m_sendfmt & SENDFMT_USERIO;
00675
00676 if(send_uio) {
00677 if(send_userio_size < send_userio.size()) {
00678 return E_FAIL;
00679 }
00680 }
00681
00682
00683 recv_uio = m_recvfmt & RECVFMT_USERIO;
00684
00685
00686 int num = 1 + send_hio + send_mio + 3 * send_uio + 2 * recv_uio;
00687
00688 VARIANT *pvnt;
00689 double *pdbl;
00690 uint8_t *pbool;
00691
00692 if(num == 1) {
00693
00694 send->vt = (VT_ARRAY | VT_R8);
00695 send->parray = SafeArrayCreateVector(VT_R8, 0, joints);
00696 SafeArrayAccessData(send->parray, (void**)&pdbl);
00697 memset(pdbl, 0, joints * sizeof(double));
00698 std::copy(pose.begin(), pose.end(), pdbl);
00699 SafeArrayUnaccessData(send->parray);
00700 } else {
00701 send->vt = (VT_ARRAY | VT_VARIANT);
00702 send->parray = SafeArrayCreateVector(VT_VARIANT, 0, num);
00703
00704 SafeArrayAccessData(send->parray, (void**)&pvnt);
00705
00706 int offset = 0;
00707
00708
00709 {
00710 pvnt[offset].vt = (VT_ARRAY | VT_R8);
00711 pvnt[offset].parray = SafeArrayCreateVector(VT_R8, 0, joints);
00712 SafeArrayAccessData(pvnt[offset].parray, (void**)&pdbl);
00713 memset(pdbl, 0, joints * sizeof(double));
00714 std::copy(pose.begin(), pose.end(), pdbl);
00715 SafeArrayUnaccessData(pvnt[offset].parray);
00716
00717 offset++;
00718 }
00719
00720
00721 if(send_mio) {
00722 pvnt[offset].vt = VT_I4;
00723 pvnt[offset].lVal = miniio;
00724
00725 offset++;
00726 }
00727
00728
00729 if(send_uio) {
00730 pvnt[offset+0].vt = VT_I4;
00731 pvnt[offset+0].lVal = send_userio_offset;
00732
00733 pvnt[offset+1].vt = VT_I4;
00734 pvnt[offset+1].lVal = send_userio_size * UserIO::USERIO_ALIGNMENT;
00735
00736 pvnt[offset+2].vt = (VT_ARRAY | VT_UI1);
00737 pvnt[offset+2].parray = SafeArrayCreateVector(VT_UI1, 0, send_userio_size);
00738 SafeArrayAccessData(pvnt[offset+2].parray, (void**)&pbool);
00739 memset(pbool, 0, send_userio_size * sizeof(uint8_t));
00740 std::copy(send_userio.begin(), send_userio.end(), pbool);
00741 SafeArrayUnaccessData(pvnt[offset+2].parray);
00742
00743 offset+=3;
00744 }
00745
00746
00747 if(recv_uio) {
00748 pvnt[offset+0].vt = VT_I4;
00749 pvnt[offset+0].lVal = recv_userio_offset;
00750
00751 pvnt[offset+1].vt = VT_I4;
00752 pvnt[offset+1].lVal = recv_userio_size * UserIO::USERIO_ALIGNMENT;
00753
00754 offset+=2;
00755 }
00756
00757
00758 if(send_hio) {
00759 pvnt[offset].vt = VT_I4;
00760 pvnt[offset].lVal = handio;
00761
00762 offset++;
00763 }
00764
00765 SafeArrayUnaccessData(send->parray);
00766 }
00767
00768 return S_OK;
00769 }
00770
00771 HRESULT DensoRobotRC8::ParseRecvParameter(
00772 const VARIANT_Ptr& recv,
00773 std::vector<double>& position, std::vector<double>& joint, std::vector<double>& trans,
00774 int& miniio, int& handio, int& timestamp,
00775 std::vector<uint8_t>& recv_userio,
00776 std::vector<double>& current)
00777 {
00778 int type = m_recvfmt & SLVMODE_POSE;
00779
00780
00781 int j = 0, j1 = 0, j2 = 0, joints = 0;
00782 std::vector<double> *pose1 = NULL, *pose2 = NULL;
00783
00784 switch(type) {
00785 case RECVFMT_POSE_P:
00786 j1 = NUM_POSITION;
00787 pose1 = &position;
00788 break;
00789 case RECVFMT_POSE_J:
00790 j1 = NUM_JOINT;
00791 pose1 = &joint;
00792 break;
00793 case RECVFMT_POSE_T:
00794 j1 = NUM_TRANS;
00795 pose1 = &trans;
00796 break;
00797 case RECVFMT_POSE_PJ:
00798 j1 = NUM_POSITION;
00799 j2 = NUM_JOINT;
00800 pose1 = &position;
00801 pose2 = &joint;
00802 break;
00803 case RECVFMT_POSE_TJ:
00804 j1 = NUM_TRANS;
00805 j2 = NUM_JOINT;
00806 pose1 = &trans;
00807 pose2 = &joint;
00808 break;
00809 default:
00810 return E_FAIL;
00811 }
00812
00813 joints = j1 + j2;
00814
00815
00816 bool recv_ts, recv_hio, recv_mio, recv_uio, recv_crt;
00817 recv_ts = m_recvfmt & RECVFMT_TIME;
00818 recv_hio = m_recvfmt & RECVFMT_HANDIO;
00819 recv_mio = m_recvfmt & RECVFMT_MINIIO;
00820 recv_uio = m_recvfmt & RECVFMT_USERIO;
00821 recv_crt = m_recvfmt & RECVFMT_CURRENT;
00822
00823
00824 int num = 1 + recv_ts + recv_hio + recv_mio + recv_uio + recv_crt;
00825
00826 HRESULT hr = S_OK;
00827 VARIANT *pvnt;
00828 double *pdbl;
00829 uint8_t *pbool;
00830
00831 if(recv->vt == (VT_ARRAY | VT_R8)) {
00832 if(joints != recv->parray->rgsabound->cElements) {
00833 return E_FAIL;
00834 }
00835
00836
00837 SafeArrayAccessData(recv->parray, (void**)&pdbl);
00838 pose1->resize(j1);
00839 std::copy(pdbl, &pdbl[j1], pose1->begin());
00840 if(pose2 != NULL) {
00841 pose2->resize(j2);
00842 std::copy(&pdbl[j1], &pdbl[joints], pose2->begin());
00843 }
00844 SafeArrayUnaccessData(recv->parray);
00845 }
00846 else if(recv->vt == (VT_ARRAY | VT_VARIANT)) {
00847 if(num != recv->parray->rgsabound->cElements) {
00848 return E_FAIL;
00849 }
00850
00851 SafeArrayAccessData(recv->parray, (void**)&pvnt);
00852
00853 int offset = 0;
00854
00855
00856 if(recv_ts) {
00857 if(pvnt[offset].vt != VT_I4) {
00858 hr = E_FAIL;
00859 goto exit_proc;
00860 }
00861
00862 timestamp = pvnt[offset].lVal;
00863
00864 offset++;
00865 }
00866
00867
00868 {
00869 if((pvnt[offset].vt != (VT_ARRAY | VT_R8))
00870 || (joints != pvnt[offset].parray->rgsabound->cElements))
00871 {
00872 hr = E_FAIL;
00873 goto exit_proc;
00874 }
00875
00876 SafeArrayAccessData(pvnt[offset].parray, (void**)&pdbl);
00877 pose1->resize(j1);
00878 std::copy(pdbl, &pdbl[j1], pose1->begin());
00879 if(pose2 != NULL) {
00880 pose2->resize(j2);
00881 std::copy(&pdbl[j1], &pdbl[joints], pose2->begin());
00882 }
00883 SafeArrayUnaccessData(pvnt[offset].parray);
00884
00885 offset++;
00886 }
00887
00888
00889 if(recv_mio) {
00890 if(pvnt[offset].vt != VT_I4) {
00891 hr = E_FAIL;
00892 goto exit_proc;
00893 }
00894
00895 miniio = pvnt[offset].lVal;
00896
00897 offset++;
00898 }
00899
00900
00901 if(recv_uio) {
00902 if(pvnt[offset].vt != (VT_ARRAY | VT_UI1))
00903 {
00904 hr = E_FAIL;
00905 goto exit_proc;
00906 }
00907
00908 SafeArrayAccessData(pvnt[offset].parray, (void**)&pbool);
00909 recv_userio.resize(pvnt[offset].parray->rgsabound->cElements);
00910 std::copy(pbool, &pbool[pvnt[offset].parray->rgsabound->cElements], recv_userio.begin());
00911 SafeArrayUnaccessData(pvnt[offset].parray);
00912
00913 offset++;
00914 }
00915
00916
00917 if(recv_hio) {
00918 if(pvnt[offset].vt != VT_I4) {
00919 hr = E_FAIL;
00920 goto exit_proc;
00921 }
00922
00923 handio = pvnt[offset].lVal;
00924
00925 offset++;
00926 }
00927
00928
00929 if(recv_crt) {
00930 if((pvnt[offset].vt != (VT_ARRAY | VT_R8))
00931 || (8 != pvnt[offset].parray->rgsabound->cElements))
00932 {
00933 hr = E_FAIL;
00934 goto exit_proc;
00935 }
00936
00937 SafeArrayAccessData(pvnt[offset].parray, (void**)&pdbl);
00938 current.resize(8);
00939 std::copy(pdbl, &pdbl[8], current.begin());
00940 SafeArrayUnaccessData(pvnt[offset].parray);
00941
00942 offset++;
00943 }
00944
00945 exit_proc:
00946 SafeArrayUnaccessData(recv->parray);
00947 } else {
00948 return E_FAIL;
00949 }
00950
00951 return hr;
00952 }
00953
00954 void DensoRobotRC8::Callback_MoveString(const MoveStringGoalConstPtr& goal)
00955 {
00956 HRESULT hr;
00957 MoveStringResult res;
00958
00959
00960 boost::mutex::scoped_lock lockAct(m_mtxAct);
00961 if(m_curAct != ACT_NONE) {
00962 if(m_curAct != ACT_RESET) {
00963 res.HRESULT = E_FAIL;
00964 m_actMoveString->setAborted(res);
00965 }
00966 return;
00967 }
00968 m_curAct = ACT_MOVESTRING;
00969 lockAct.unlock();
00970
00971 VARIANT_Ptr vntPose(new VARIANT());
00972 VariantInit(vntPose.get());
00973 vntPose->vt = VT_BSTR;
00974 vntPose->bstrVal = ConvertStringToBSTR(goal->pose);
00975
00976 hr = ExecMove(goal->comp, vntPose, goal->option);
00977
00978
00979 m_mtxAct.lock();
00980 if(m_curAct == ACT_MOVESTRING) {
00981 if(SUCCEEDED(hr)) {
00982 res.HRESULT = S_OK;
00983 m_actMoveString->setSucceeded(res);
00984 } else {
00985 res.HRESULT = hr;
00986 m_actMoveString->setAborted(res);
00987 }
00988 m_curAct = ACT_NONE;
00989 }
00990 m_mtxAct.unlock();
00991 }
00992
00993 void DensoRobotRC8::Callback_MoveValue(const MoveValueGoalConstPtr& goal)
00994 {
00995 HRESULT hr;
00996 MoveValueResult res;
00997
00998
00999 boost::mutex::scoped_lock lockAct(m_mtxAct);
01000 if(m_curAct != ACT_NONE) {
01001 if(m_curAct != ACT_RESET) {
01002 res.HRESULT = E_FAIL;
01003 m_actMoveValue->setAborted(res);
01004 }
01005 return;
01006 }
01007 m_curAct = ACT_MOVEVALUE;
01008 lockAct.unlock();
01009
01010 VARIANT_Ptr vntPose(new VARIANT());
01011 VariantInit(vntPose.get());
01012 CreatePoseData(goal->pose, *vntPose.get());
01013
01014 hr = ExecMove(goal->comp, vntPose, goal->option);
01015
01016
01017 m_mtxAct.lock();
01018 if(m_curAct == ACT_MOVEVALUE) {
01019 if(SUCCEEDED(hr)){
01020 res.HRESULT = S_OK;
01021 m_actMoveValue->setSucceeded(res);
01022 }else{
01023 res.HRESULT = hr;
01024 m_actMoveValue->setAborted(res);
01025 }
01026 m_curAct = ACT_NONE;
01027 }
01028 m_mtxAct.unlock();
01029 }
01030
01031 void DensoRobotRC8::Callback_DriveString(
01032 const std::string& name, const DriveStringGoalConstPtr& goal)
01033 {
01034 HRESULT hr;
01035 DriveStringResult res;
01036 BSTR *pbstr;
01037
01038 int act = 0;
01039 boost::shared_ptr<SimpleActionServer<DriveStringAction> > actSvr;
01040
01041 if(!name.compare("DriveEx")) {
01042 act = ACT_DRIVEEXSTRING;
01043 actSvr = m_actDriveExString;
01044 }
01045 else if(!name.compare("DriveAEx")) {
01046 act = ACT_DRIVEAEXSTRING;
01047 actSvr = m_actDriveAExString;
01048 }
01049 else return;
01050
01051
01052 boost::mutex::scoped_lock lockAct(m_mtxAct);
01053 if(m_curAct != ACT_NONE) {
01054 if(m_curAct != ACT_RESET) {
01055 res.HRESULT = E_FAIL;
01056 actSvr->setAborted(res);
01057 }
01058 return;
01059 }
01060 m_curAct = act;
01061 lockAct.unlock();
01062
01063 VARIANT_Ptr vntOpt(new VARIANT());
01064 VariantInit(vntOpt.get());
01065 vntOpt->vt = (VT_ARRAY | VT_BSTR);
01066 vntOpt->parray = SafeArrayCreateVector(VT_BSTR, 0, 2);
01067 SafeArrayAccessData(vntOpt->parray, (void**)&pbstr);
01068 pbstr[0] = ConvertStringToBSTR(goal->pose);
01069 pbstr[1] = ConvertStringToBSTR(goal->option);
01070 SafeArrayUnaccessData(vntOpt->parray);
01071
01072 hr = ExecDrive(name, vntOpt);
01073
01074
01075 m_mtxAct.lock();
01076 if(m_curAct == act) {
01077 if(SUCCEEDED(hr)) {
01078 res.HRESULT = S_OK;
01079 actSvr->setSucceeded(res);
01080 } else {
01081 res.HRESULT = hr;
01082 actSvr->setAborted(res);
01083 }
01084 m_curAct = ACT_NONE;
01085 }
01086 m_mtxAct.unlock();
01087 }
01088
01089 void DensoRobotRC8::Callback_DriveValue(
01090 const std::string& name, const DriveValueGoalConstPtr& goal)
01091 {
01092 HRESULT hr;
01093 DriveValueResult res;
01094 VARIANT *pvntval, *pvntjnt;
01095
01096 int act = 0;
01097 boost::shared_ptr<SimpleActionServer<DriveValueAction> > actSvr;
01098
01099 if(!name.compare("DriveEx")) {
01100 act = ACT_DRIVEEXVALUE;
01101 actSvr = m_actDriveExValue;
01102 }
01103 else if(!name.compare("DriveAEx")) {
01104 act = ACT_DRIVEAEXVALUE;
01105 actSvr = m_actDriveAExValue;
01106 }
01107 else return;
01108
01109
01110 boost::mutex::scoped_lock lockAct(m_mtxAct);
01111 if(m_curAct != ACT_NONE) {
01112 if(m_curAct != ACT_RESET) {
01113 res.HRESULT = E_FAIL;
01114 actSvr->setAborted(res);
01115 }
01116 return;
01117 }
01118 m_curAct = act;
01119 lockAct.unlock();
01120
01121 VARIANT_Ptr vntOpt(new VARIANT());
01122 VariantInit(vntOpt.get());
01123
01124 vntOpt->vt = (VT_ARRAY | VT_VARIANT);
01125 vntOpt->parray = SafeArrayCreateVector(VT_VARIANT, 0, 2);
01126
01127 SafeArrayAccessData(vntOpt->parray, (void**)&pvntval);
01128
01129 pvntval[0].vt = (VT_ARRAY | VT_VARIANT);
01130 pvntval[0].parray = SafeArrayCreateVector(VT_VARIANT, 0, goal->pose.size());
01131
01132 SafeArrayAccessData(pvntval[0].parray, (void**)&pvntjnt);
01133
01134 for(int i = 0; i < goal->pose.size(); i++) {
01135 PoseData pd;
01136 pd.value.push_back(goal->pose.at(i).joint);
01137 pd.value.push_back(goal->pose.at(i).value);
01138 pd.type = -1;
01139 pd.pass = (i == 0) ? goal->pass : 0;
01140 CreatePoseData(pd, pvntjnt[i]);
01141 }
01142
01143 SafeArrayUnaccessData(pvntval[0].parray);
01144
01145 pvntval[1].vt = VT_BSTR;
01146 pvntval[1].bstrVal = ConvertStringToBSTR(goal->option);
01147
01148 SafeArrayUnaccessData(vntOpt->parray);
01149
01150 hr = ExecDrive(name, vntOpt);
01151
01152
01153 m_mtxAct.lock();
01154 if(m_curAct == act) {
01155 if(SUCCEEDED(hr)) {
01156 res.HRESULT = S_OK;
01157 actSvr->setSucceeded(res);
01158 }else{
01159 res.HRESULT = hr;
01160 actSvr->setAborted(res);
01161 }
01162 m_curAct = ACT_NONE;
01163 }
01164 m_mtxAct.unlock();
01165 }
01166
01167 void DensoRobotRC8::Callback_Speed(const Float32::ConstPtr& msg)
01168 {
01169 ExecSpeed(msg->data);
01170 }
01171
01172 void DensoRobotRC8::Callback_Change(
01173 const std::string& name, const Int32::ConstPtr& msg)
01174 {
01175 std::stringstream ss;
01176 ss << name << msg->data;
01177 ExecChange(ss.str());
01178 }
01179
01180 void DensoRobotRC8::Callback_Cancel()
01181 {
01182 boost::mutex::scoped_lock lockAct(m_mtxAct);
01183
01184 if(m_curAct > ACT_NONE) {
01185 ExecHalt();
01186
01187 switch(m_curAct){
01188 case ACT_MOVESTRING:
01189 m_actMoveString->setPreempted();
01190 break;
01191 case ACT_MOVEVALUE:
01192 m_actMoveValue->setPreempted();
01193 break;
01194 case ACT_DRIVEEXSTRING:
01195 m_actDriveExString->setPreempted();
01196 break;
01197 case ACT_DRIVEEXVALUE:
01198 m_actDriveExValue->setPreempted();
01199 break;
01200 case ACT_DRIVEAEXSTRING:
01201 m_actDriveAExString->setPreempted();
01202 break;
01203 case ACT_DRIVEAEXVALUE:
01204 m_actDriveAExValue->setPreempted();
01205 break;
01206 }
01207
01208 m_curAct = ACT_NONE;
01209 }
01210 }
01211
01212 void DensoRobotRC8::Action_Feedback()
01213 {
01214 boost::mutex::scoped_lock lockAct(m_mtxAct);
01215
01216 if(m_curAct > ACT_NONE) {
01217 HRESULT hr;
01218 std::vector<double> pose;
01219
01220 MoveStringFeedback fbMvStr;
01221 MoveValueFeedback fbMvVal;
01222 DriveStringFeedback fbDrvStr;
01223 DriveValueFeedback fbDrvVal;
01224
01225 hr = ExecCurJnt(pose);
01226
01227 if(SUCCEEDED(hr)) {
01228 switch(m_curAct){
01229 case ACT_MOVESTRING:
01230 fbMvStr.pose = pose;
01231 m_actMoveString->publishFeedback(fbMvStr);
01232 break;
01233 case ACT_MOVEVALUE:
01234 fbMvVal.pose = pose;
01235 m_actMoveValue->publishFeedback(fbMvVal);
01236 break;
01237 case ACT_DRIVEEXSTRING:
01238 fbDrvStr.pose = pose;
01239 m_actDriveExString->publishFeedback(fbDrvStr);
01240 break;
01241 case ACT_DRIVEEXVALUE:
01242 fbDrvVal.pose = pose;
01243 m_actDriveExValue->publishFeedback(fbDrvVal);
01244 break;
01245 case ACT_DRIVEAEXSTRING:
01246 fbDrvStr.pose = pose;
01247 m_actDriveAExString->publishFeedback(fbDrvStr);
01248 break;
01249 case ACT_DRIVEAEXVALUE:
01250 fbDrvVal.pose = pose;
01251 m_actDriveAExValue->publishFeedback(fbDrvVal);
01252 break;
01253 }
01254 }
01255 }
01256 }
01257
01258 int DensoRobotRC8::get_SendFormat() const
01259 {
01260 return m_sendfmt;
01261 }
01262
01263 void DensoRobotRC8::put_SendFormat(int format)
01264 {
01265 switch(format) {
01266 case SENDFMT_NONE:
01267 case SENDFMT_HANDIO:
01268 case SENDFMT_MINIIO:
01269 case SENDFMT_HANDIO | SENDFMT_MINIIO:
01270 case SENDFMT_USERIO:
01271 case SENDFMT_USERIO | SENDFMT_HANDIO:
01272 m_sendfmt = format;
01273 break;
01274 default:
01275 ROS_WARN("Failed to put_SendFormat.");
01276 break;
01277 }
01278 }
01279
01280 int DensoRobotRC8::get_RecvFormat() const
01281 {
01282 return m_recvfmt;
01283 }
01284
01285 void DensoRobotRC8::put_RecvFormat(int format)
01286 {
01287 int pose = format & RECVFMT_POSE;
01288 if((RECVFMT_NONE <= pose)
01289 && (pose <= RECVFMT_POSE_TJ))
01290 {
01291 switch(format & ~RECVFMT_POSE) {
01292 case RECVFMT_NONE:
01293 case RECVFMT_TIME:
01294 case RECVFMT_HANDIO:
01295 case RECVFMT_CURRENT:
01296 case RECVFMT_MINIIO:
01297 case RECVFMT_USERIO:
01298 case RECVFMT_TIME | RECVFMT_HANDIO:
01299 case RECVFMT_TIME | RECVFMT_MINIIO:
01300 case RECVFMT_HANDIO | RECVFMT_MINIIO:
01301 case RECVFMT_TIME | RECVFMT_HANDIO | RECVFMT_MINIIO:
01302 case RECVFMT_TIME | RECVFMT_USERIO:
01303 case RECVFMT_HANDIO | RECVFMT_USERIO:
01304 case RECVFMT_TIME | RECVFMT_HANDIO | RECVFMT_USERIO:
01305 case RECVFMT_CURRENT | RECVFMT_MINIIO:
01306 case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_MINIIO:
01307 case RECVFMT_CURRENT | RECVFMT_HANDIO:
01308 case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_HANDIO:
01309 case RECVFMT_CURRENT | RECVFMT_HANDIO | RECVFMT_MINIIO:
01310 case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_HANDIO | RECVFMT_MINIIO:
01311 case RECVFMT_CURRENT | RECVFMT_USERIO:
01312 case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_USERIO:
01313 case RECVFMT_CURRENT | RECVFMT_MINIIO | RECVFMT_USERIO:
01314 case RECVFMT_TIME | RECVFMT_CURRENT | RECVFMT_HANDIO | RECVFMT_USERIO:
01315 m_recvfmt = format;
01316 break;
01317 default:
01318 ROS_WARN("Failed to put_RecvFormat.");
01319 break;
01320 }
01321 }
01322 else
01323 {
01324 ROS_WARN("Failed to put_RecvFormat.");
01325 }
01326 }
01327
01328 int DensoRobotRC8::get_TimeFormat() const
01329 {
01330 return m_tsfmt;
01331 }
01332
01333 void DensoRobotRC8::put_TimeFormat(int format)
01334 {
01335 if((format == TSFMT_MILLISEC)
01336 || (format == TSFMT_MICROSEC))
01337 {
01338 m_tsfmt = format;
01339 }
01340 else
01341 {
01342 ROS_WARN("Failed to put_TimeFormat.");
01343 }
01344 }
01345
01346 int DensoRobotRC8::get_MiniIO() const
01347 {
01348 return m_recv_miniio;
01349 }
01350
01351 void DensoRobotRC8::put_MiniIO(int value)
01352 {
01353 m_send_miniio = value;
01354 }
01355
01356 int DensoRobotRC8::get_HandIO() const
01357 {
01358 return m_recv_handio;
01359 }
01360
01361 void DensoRobotRC8::put_HandIO(int value)
01362 {
01363 m_send_handio = value;
01364 }
01365
01366 void DensoRobotRC8::put_SendUserIO(const UserIO& value)
01367 {
01368 if(value.offset < UserIO::MIN_USERIO_OFFSET)
01369 {
01370 ROS_WARN("User I/O offset has to be greater than %d.",
01371 UserIO::MIN_USERIO_OFFSET - 1);
01372 return;
01373 }
01374
01375 if(value.offset % UserIO::USERIO_ALIGNMENT)
01376 {
01377 ROS_WARN("User I/O offset has to be multiple of %d.",
01378 UserIO::USERIO_ALIGNMENT);
01379 return;
01380 }
01381
01382 if(value.size <= 0)
01383 {
01384 ROS_WARN("User I/O size has to be greater than 0.");
01385 return;
01386 }
01387
01388 if(value.size < value.value.size())
01389 {
01390 ROS_WARN("User I/O size has to be equal or greater than the value length.");
01391 return;
01392 }
01393
01394 m_send_userio_offset = value.offset;
01395 m_send_userio_size = value.size;
01396 m_send_userio = value.value;
01397 }
01398
01399 void DensoRobotRC8::put_RecvUserIO(const UserIO& value)
01400 {
01401 if(value.offset < UserIO::MIN_USERIO_OFFSET)
01402 {
01403 ROS_WARN("User I/O offset has to be greater than %d.",
01404 UserIO::MIN_USERIO_OFFSET - 1);
01405 return;
01406 }
01407
01408 if(value.offset % UserIO::USERIO_ALIGNMENT)
01409 {
01410 ROS_WARN("User I/O offset has to be multiple of %d.",
01411 UserIO::USERIO_ALIGNMENT);
01412 return;
01413 }
01414
01415 if(value.size <= 0)
01416 {
01417 ROS_WARN("User I/O size has to be greater than 0.");
01418 return;
01419 }
01420
01421 m_recv_userio_offset = value.offset;
01422 m_recv_userio_size = value.size;
01423 }
01424
01425 void DensoRobotRC8::get_RecvUserIO(UserIO& value) const
01426 {
01427 value.offset = m_recv_userio_offset;
01428 value.size = m_recv_userio.size();
01429 value.value = m_recv_userio;
01430 }
01431
01432 void DensoRobotRC8::get_Current(std::vector<double> ¤t) const
01433 {
01434 current = m_current;
01435 }
01436
01437 int DensoRobotRC8::get_Timestamp() const
01438 {
01439 return m_timestamp;
01440 }
01441
01442 }