00001
00025 #include <stdlib.h>
00026 #include <sstream>
00027 #include "bcap_service/bcap_service.h"
00028 #include "bcap_core/bcap_funcid.h"
00029 #include "bcap_core/bCAPClient/bcap_client.h"
00030 #include "bcap_core/RACString/rac_string.h"
00031
00032 int main(int argc, char** argv)
00033 {
00034 ros::init(argc, argv, "bcap_service");
00035
00036 HRESULT hr;
00037 ros::NodeHandle node;
00038
00039 bcap_service::BCAPService bcapsrv;
00040
00041 bcapsrv.parseParams();
00042
00043 hr = bcapsrv.Connect();
00044 if(FAILED(hr)) {
00045 ROS_ERROR("Failed to connect. (%X)", hr);
00046 return 1;
00047 } else {
00048 bcapsrv.StartService(node);
00049
00050 ros::spin();
00051
00052 bcapsrv.StopService();
00053 bcapsrv.Disconnect();
00054 return 0;
00055 }
00056 }
00057
00058 namespace bcap_service {
00059
00060 BCAPService::BCAPService()
00061 : m_type(""), m_addr(""),
00062 m_port(0), m_timeout(0), m_retry(0), m_wait(0),
00063 m_fd(0), m_wdt(0), m_invoke(0)
00064 {
00065
00066 }
00067
00068 BCAPService::~BCAPService()
00069 {
00070 StopService();
00071 Disconnect();
00072 }
00073
00074 void BCAPService::parseParams()
00075 {
00076 ros::NodeHandle node;
00077
00078 if(!node.getParam("conn_type", m_type))
00079 {
00080 m_type = "tcp";
00081 }
00082
00083 if(!node.getParam("ip_address", m_addr))
00084 {
00085 m_addr = "192.168.0.1";
00086 }
00087
00088 if(!node.getParam("port_number", m_port))
00089 {
00090 m_port = 5007;
00091 }
00092
00093 if(!node.getParam("timeout", m_timeout))
00094 {
00095 m_timeout = 3000;
00096 }
00097
00098 if(!node.getParam("retry_count", m_retry))
00099 {
00100 m_retry = 5;
00101 }
00102
00103 if(!node.getParam("wait_time", m_wait))
00104 {
00105 m_wait = 0;
00106 }
00107
00108 if(!node.getParam("watchdog_timer", m_wdt))
00109 {
00110 m_wdt = 400;
00111 }
00112
00113 if(!node.getParam("invoke_timeout", m_invoke))
00114 {
00115 m_invoke = 180000;
00116 }
00117 }
00118
00119 HRESULT BCAPService::Connect()
00120 {
00121 HRESULT hr;
00122 std::stringstream ss1;
00123 std::wstringstream ss2;
00124
00125 ros::Duration(m_wait).sleep();
00126
00127 ss1 << m_type << ":" << m_addr << ":" << m_port;
00128 hr = bCap_Open_Client(ss1.str().c_str(), m_timeout, m_retry, &m_fd);
00129 if(SUCCEEDED(hr)) {
00130 ss2 << L",WDT=" << m_wdt << L",InvokeTimeout=" << m_invoke;
00131 BSTR bstrOption = SysAllocString(ss2.str().c_str());
00132 hr = bCap_ServiceStart(m_fd, bstrOption);
00133 SysFreeString(bstrOption);
00134 }
00135
00136 return hr;
00137 }
00138
00139 HRESULT BCAPService::Disconnect()
00140 {
00141 if(m_fd > 0) {
00142 KeyHandle_Vec::iterator it;
00143 for(it = m_vecKH.begin(); it != m_vecKH.end(); it++) {
00144 switch(it->first){
00145 case ID_CONTROLLER_DISCONNECT:
00146 bCap_ControllerDisconnect(m_fd, &it->second);
00147 break;
00148 case ID_EXTENSION_RELEASE:
00149 bCap_ExtensionRelease(m_fd, &it->second);
00150 break;
00151 case ID_FILE_RELEASE:
00152 bCap_FileRelease(m_fd, &it->second);
00153 break;
00154 case ID_ROBOT_RELEASE:
00155 bCap_RobotRelease(m_fd, &it->second);
00156 break;
00157 case ID_TASK_RELEASE:
00158 bCap_TaskRelease(m_fd, &it->second);
00159 break;
00160 case ID_VARIABLE_RELEASE:
00161 bCap_VariableRelease(m_fd, &it->second);
00162 break;
00163 case ID_COMMAND_RELEASE:
00164 bCap_CommandRelease(m_fd, &it->second);
00165 break;
00166 case ID_MESSAGE_RELEASE:
00167 bCap_MessageRelease(m_fd, &it->second);
00168 break;
00169 }
00170 }
00171
00172 m_vecKH.clear();
00173
00174 bCap_ServiceStop(m_fd);
00175
00176 bCap_Close_Client(&m_fd);
00177 }
00178
00179 return S_OK;
00180 }
00181
00182 HRESULT BCAPService::StartService(ros::NodeHandle& node)
00183 {
00184 m_svr = node.advertiseService("bcap_service", &BCAPService::CallFunction, this);
00185 return S_OK;
00186 }
00187
00188 HRESULT BCAPService::StopService()
00189 {
00190 m_svr.shutdown();
00191 return S_OK;
00192 }
00193
00194 const std::string& BCAPService::get_Type() const
00195 {
00196 return m_type;
00197 }
00198
00199 void BCAPService::put_Type(const std::string& type)
00200 {
00201 if(m_fd == 0) {
00202 m_type = type;
00203 }
00204 }
00205
00206 uint32_t BCAPService::get_Timeout() const
00207 {
00208 uint32_t value = 0;
00209 if(FAILED(bCap_GetTimeout(m_fd, &value))) {
00210 value = m_timeout;
00211 }
00212 return value;
00213 }
00214
00215 void BCAPService::put_Timeout(uint32_t value)
00216 {
00217 if(SUCCEEDED(bCap_SetTimeout(m_fd, value))) {
00218 m_timeout = value;
00219 }
00220 }
00221
00222 unsigned int BCAPService::get_Retry() const
00223 {
00224 unsigned int value = 0;
00225 if(FAILED(bCap_GetRetry(m_fd, &value))) {
00226 value = m_retry;
00227 }
00228 return value;
00229 }
00230
00231 void BCAPService::put_Retry(unsigned int value)
00232 {
00233 if(SUCCEEDED(bCap_SetRetry(m_fd, value))) {
00234 m_retry = value;
00235 }
00236 }
00237
00238 bool BCAPService::CallFunction(bcap::Request &req, bcap::Response &res)
00239 {
00240 HRESULT hr = S_OK;
00241 char *chRet = NULL;
00242 VARIANT_Vec vntArgs;
00243 VARIANT_Ptr vntRet(new VARIANT());
00244 VariantInit(vntRet.get());
00245
00246 for(int i = 0; i < req.vntArgs.size(); i++) {
00247 VARIANT_Ptr vntTmp(new VARIANT());
00248 VariantInit(vntTmp.get());
00249 hr = ConvertRacStr2Variant(
00250 req.vntArgs[i].vt, req.vntArgs[i].value.c_str(),
00251 vntTmp.get());
00252 if(FAILED(hr)) goto err_proc;
00253 vntArgs.push_back(*vntTmp.get());
00254 }
00255
00256 hr = ExecFunction(req.func_id, vntArgs, vntRet);
00257 if(FAILED(hr)) goto err_proc;
00258
00259 hr = ConvertVariant2RacStr(*vntRet.get(), &chRet);
00260 if(FAILED(hr)) goto err_proc;
00261
00262 res.HRESULT = S_OK;
00263 res.vntRet.vt = vntRet->vt;
00264 res.vntRet.value = chRet;
00265
00266 free(chRet);
00267
00268 post_proc:
00269 return true;
00270
00271 err_proc:
00272 res.HRESULT = hr;
00273 res.vntRet.vt = VT_EMPTY;
00274 res.vntRet.value = "";
00275
00276 goto post_proc;
00277 }
00278
00279 HRESULT BCAPService::ExecFunction(
00280 int32_t func_id, VARIANT_Vec& vntArgs,
00281 VARIANT_Ptr& vntRet)
00282 {
00283 HRESULT hr = E_INVALIDARG;
00284 KeyHandle tmpKH(0, 0);
00285
00286 try {
00287 switch(func_id) {
00288
00289 case ID_CONTROLLER_CONNECT:
00290 vntRet->vt = VT_UI4;
00291 hr = bCap_ControllerConnect(m_fd,
00292 vntArgs.at(0).bstrVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal, vntArgs.at(3).bstrVal,
00293 &vntRet->ulVal);
00294 tmpKH.first = ID_CONTROLLER_DISCONNECT;
00295 break;
00296 case ID_CONTROLLER_DISCONNECT:
00297 tmpKH.second = vntArgs.at(0).ulVal;
00298 hr = bCap_ControllerDisconnect(m_fd,
00299 &vntArgs.at(0).ulVal);
00300 break;
00301 case ID_CONTROLLER_GETEXTENSION:
00302 vntRet->vt = VT_UI4;
00303 hr = bCap_ControllerGetExtension(m_fd,
00304 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00305 &vntRet->ulVal);
00306 tmpKH.first = ID_EXTENSION_RELEASE;
00307 break;
00308 case ID_CONTROLLER_GETFILE:
00309 vntRet->vt = VT_UI4;
00310 hr = bCap_ControllerGetFile(m_fd,
00311 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00312 &vntRet->ulVal);
00313 tmpKH.first = ID_FILE_RELEASE;
00314 break;
00315 case ID_CONTROLLER_GETROBOT:
00316 vntRet->vt = VT_UI4;
00317 hr = bCap_ControllerGetRobot(m_fd,
00318 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00319 &vntRet->ulVal);
00320 tmpKH.first = ID_ROBOT_RELEASE;
00321 break;
00322 case ID_CONTROLLER_GETTASK:
00323 vntRet->vt = VT_UI4;
00324 hr = bCap_ControllerGetTask(m_fd,
00325 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00326 &vntRet->ulVal);
00327 tmpKH.first = ID_TASK_RELEASE;
00328 break;
00329 case ID_CONTROLLER_GETVARIABLE:
00330 vntRet->vt = VT_UI4;
00331 hr = bCap_ControllerGetVariable(m_fd,
00332 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00333 &vntRet->ulVal);
00334 tmpKH.first = ID_VARIABLE_RELEASE;
00335 break;
00336 case ID_CONTROLLER_GETCOMMAND:
00337 vntRet->vt = VT_UI4;
00338 hr = bCap_ControllerGetCommand(m_fd,
00339 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00340 &vntRet->ulVal);
00341 tmpKH.first = ID_COMMAND_RELEASE;
00342 break;
00343 case ID_CONTROLLER_GETEXTENSIONNAMES:
00344 hr = bCap_ControllerGetExtensionNames(m_fd,
00345 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00346 vntRet.get());
00347 break;
00348 case ID_CONTROLLER_GETFILENAMES:
00349 hr = bCap_ControllerGetFileNames(m_fd,
00350 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00351 vntRet.get());
00352 break;
00353 case ID_CONTROLLER_GETROBOTNAMES:
00354 hr = bCap_ControllerGetRobotNames(m_fd,
00355 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00356 vntRet.get());
00357 break;
00358 case ID_CONTROLLER_GETTASKNAMES:
00359 hr = bCap_ControllerGetTaskNames(m_fd,
00360 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00361 vntRet.get());
00362 break;
00363 case ID_CONTROLLER_GETVARIABLENAMES:
00364 hr = bCap_ControllerGetVariableNames(m_fd,
00365 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00366 vntRet.get());
00367 break;
00368 case ID_CONTROLLER_GETCOMMANDNAMES:
00369 hr = bCap_ControllerGetCommandNames(m_fd,
00370 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00371 vntRet.get());
00372 break;
00373 case ID_CONTROLLER_EXECUTE:
00374 hr = bCap_ControllerExecute(m_fd,
00375 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2),
00376 vntRet.get());
00377 break;
00378 case ID_CONTROLLER_GETMESSAGE:
00379 vntRet->vt = VT_UI4;
00380 hr = bCap_ControllerGetMessage(m_fd,
00381 vntArgs.at(0).ulVal,
00382 &vntRet->ulVal);
00383 tmpKH.first = ID_MESSAGE_RELEASE;
00384 break;
00385 case ID_CONTROLLER_GETATTRIBUTE:
00386 vntRet->vt = VT_I4;
00387 hr = bCap_ControllerGetAttribute(m_fd,
00388 vntArgs.at(0).ulVal,
00389 &vntRet->lVal);
00390 break;
00391 case ID_CONTROLLER_GETHELP:
00392 vntRet->vt = VT_BSTR;
00393 hr = bCap_ControllerGetHelp(m_fd,
00394 vntArgs.at(0).ulVal,
00395 &vntRet->bstrVal);
00396 break;
00397 case ID_CONTROLLER_GETNAME:
00398 vntRet->vt = VT_BSTR;
00399 hr = bCap_ControllerGetName(m_fd,
00400 vntArgs.at(0).ulVal,
00401 &vntRet->bstrVal);
00402 break;
00403 case ID_CONTROLLER_GETTAG:
00404 hr = bCap_ControllerGetTag(m_fd,
00405 vntArgs.at(0).ulVal,
00406 vntRet.get());
00407 break;
00408 case ID_CONTROLLER_PUTTAG:
00409 hr = bCap_ControllerPutTag(m_fd,
00410 vntArgs.at(0).ulVal, vntArgs.at(1));
00411 break;
00412 case ID_CONTROLLER_GETID:
00413 hr = bCap_ControllerGetID(m_fd,
00414 vntArgs.at(0).ulVal,
00415 vntRet.get());
00416 break;
00417 case ID_CONTROLLER_PUTID:
00418 hr = bCap_ControllerPutID(m_fd,
00419 vntArgs.at(0).ulVal, vntArgs.at(1));
00420 break;
00421
00422 case ID_EXTENSION_GETVARIABLE:
00423 vntRet->vt = VT_UI4;
00424 hr = bCap_ExtensionGetVariable(m_fd,
00425 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00426 &vntRet->ulVal);
00427 tmpKH.first = ID_VARIABLE_RELEASE;
00428 break;
00429 case ID_EXTENSION_GETVARIABLENAMES:
00430 hr = bCap_ExtensionGetVariableNames(m_fd,
00431 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00432 vntRet.get());
00433 break;
00434 case ID_EXTENSION_EXECUTE:
00435 hr = bCap_ExtensionExecute(m_fd,
00436 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2),
00437 vntRet.get());
00438 break;
00439 case ID_EXTENSION_GETATTRIBUTE:
00440 vntRet->vt = VT_I4;
00441 hr = bCap_ExtensionGetAttribute(m_fd,
00442 vntArgs.at(0).ulVal,
00443 &vntRet->lVal);
00444 break;
00445 case ID_EXTENSION_GETHELP:
00446 vntRet->vt = VT_BSTR;
00447 hr = bCap_ExtensionGetHelp(m_fd,
00448 vntArgs.at(0).ulVal,
00449 &vntRet->bstrVal);
00450 break;
00451 case ID_EXTENSION_GETNAME:
00452 vntRet->vt = VT_BSTR;
00453 hr = bCap_ExtensionGetName(m_fd,
00454 vntArgs.at(0).ulVal,
00455 &vntRet->bstrVal);
00456 break;
00457 case ID_EXTENSION_GETTAG:
00458 hr = bCap_ExtensionGetTag(m_fd,
00459 vntArgs.at(0).ulVal,
00460 vntRet.get());
00461 break;
00462 case ID_EXTENSION_PUTTAG:
00463 hr = bCap_ExtensionPutTag(m_fd,
00464 vntArgs.at(0).ulVal, vntArgs.at(1));
00465 break;
00466 case ID_EXTENSION_GETID:
00467 hr = bCap_ExtensionGetID(m_fd,
00468 vntArgs.at(0).ulVal,
00469 vntRet.get());
00470 break;
00471 case ID_EXTENSION_PUTID:
00472 hr = bCap_ExtensionPutID(m_fd,
00473 vntArgs.at(0).ulVal, vntArgs.at(1));
00474 break;
00475 case ID_EXTENSION_RELEASE:
00476 tmpKH.second = vntArgs.at(0).ulVal;
00477 hr = bCap_ExtensionRelease(m_fd,
00478 &vntArgs.at(0).ulVal);
00479 break;
00480
00481 case ID_FILE_GETFILE:
00482 vntRet->vt = VT_UI4;
00483 hr = bCap_FileGetFile(m_fd,
00484 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00485 &vntRet->ulVal);
00486 tmpKH.first = ID_FILE_RELEASE;
00487 break;
00488 case ID_FILE_GETVARIABLE:
00489 vntRet->vt = VT_UI4;
00490 hr = bCap_FileGetVariable(m_fd,
00491 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00492 &vntRet->ulVal);
00493 tmpKH.first = ID_VARIABLE_RELEASE;
00494 break;
00495 case ID_FILE_GETFILENAMES:
00496 hr = bCap_FileGetFileNames(m_fd,
00497 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00498 vntRet.get());
00499 break;
00500 case ID_FILE_GETVARIABLENAMES:
00501 hr = bCap_FileGetVariableNames(m_fd,
00502 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00503 vntRet.get());
00504 break;
00505 case ID_FILE_EXECUTE:
00506 hr = bCap_FileExecute(m_fd,
00507 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2),
00508 vntRet.get());
00509 break;
00510 case ID_FILE_COPY:
00511 hr = bCap_FileCopy(m_fd,
00512 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal);
00513 break;
00514 case ID_FILE_DELETE:
00515 hr = bCap_FileDelete(m_fd,
00516 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00517 break;
00518 case ID_FILE_MOVE:
00519 hr = bCap_FileMove(m_fd,
00520 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal);
00521 break;
00522 case ID_FILE_RUN:
00523 vntRet->vt = VT_BSTR;
00524 hr = bCap_FileRun(m_fd,
00525 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00526 &vntRet->bstrVal);
00527 break;
00528 case ID_FILE_GETDATECREATED:
00529 hr = bCap_FileGetDateCreated(m_fd,
00530 vntArgs.at(0).ulVal,
00531 vntRet.get());
00532 break;
00533 case ID_FILE_GETDATELASTACCESSED:
00534 hr = bCap_FileGetDateLastAccessed(m_fd,
00535 vntArgs.at(0).ulVal,
00536 vntRet.get());
00537 break;
00538 case ID_FILE_GETDATELASTMODIFIED:
00539 hr = bCap_FileGetDateLastModified(m_fd,
00540 vntArgs.at(0).ulVal,
00541 vntRet.get());
00542 break;
00543 case ID_FILE_GETPATH:
00544 vntRet->vt = VT_BSTR;
00545 hr = bCap_FileGetPath(m_fd,
00546 vntArgs.at(0).ulVal,
00547 &vntRet->bstrVal);
00548 break;
00549 case ID_FILE_GETSIZE:
00550 vntRet->vt = VT_I4;
00551 hr = bCap_FileGetSize(m_fd,
00552 vntArgs.at(0).ulVal,
00553 &vntRet->lVal);
00554 break;
00555 case ID_FILE_GETTYPE:
00556 vntRet->vt = VT_BSTR;
00557 hr = bCap_FileGetType(m_fd,
00558 vntArgs.at(0).ulVal,
00559 &vntRet->bstrVal);
00560 break;
00561 case ID_FILE_GETVALUE:
00562 hr = bCap_FileGetValue(m_fd,
00563 vntArgs.at(0).ulVal,
00564 vntRet.get());
00565 break;
00566 case ID_FILE_PUTVALUE:
00567 hr = bCap_FilePutValue(m_fd,
00568 vntArgs.at(0).ulVal, vntArgs.at(1));
00569 break;
00570 case ID_FILE_GETATTRIBUTE:
00571 vntRet->vt = VT_I4;
00572 hr = bCap_FileGetAttribute(m_fd,
00573 vntArgs.at(0).ulVal,
00574 &vntRet->lVal);
00575 break;
00576 case ID_FILE_GETHELP:
00577 vntRet->vt = VT_BSTR;
00578 hr = bCap_FileGetHelp(m_fd,
00579 vntArgs.at(0).ulVal,
00580 &vntRet->bstrVal);
00581 break;
00582 case ID_FILE_GETNAME:
00583 vntRet->vt = VT_BSTR;
00584 hr = bCap_FileGetName(m_fd,
00585 vntArgs.at(0).ulVal,
00586 &vntRet->bstrVal);
00587 break;
00588 case ID_FILE_GETTAG:
00589 hr = bCap_FileGetTag(m_fd,
00590 vntArgs.at(0).ulVal,
00591 vntRet.get());
00592 break;
00593 case ID_FILE_PUTTAG:
00594 hr = bCap_FilePutTag(m_fd,
00595 vntArgs.at(0).ulVal, vntArgs.at(1));
00596 break;
00597 case ID_FILE_GETID:
00598 hr = bCap_FileGetID(m_fd,
00599 vntArgs.at(0).ulVal,
00600 vntRet.get());
00601 break;
00602 case ID_FILE_PUTID:
00603 hr = bCap_FilePutID(m_fd,
00604 vntArgs.at(0).ulVal, vntArgs.at(1));
00605 break;
00606 case ID_FILE_RELEASE:
00607 tmpKH.second = vntArgs.at(0).ulVal;
00608 hr = bCap_FileRelease(m_fd,
00609 &vntArgs.at(0).ulVal);
00610 break;
00611
00612 case ID_ROBOT_GETVARIABLE:
00613 vntRet->vt = VT_UI4;
00614 hr = bCap_RobotGetVariable(m_fd,
00615 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00616 &vntRet->ulVal);
00617 tmpKH.first = ID_VARIABLE_RELEASE;
00618 break;
00619 case ID_ROBOT_GETVARIABLENAMES:
00620 hr = bCap_RobotGetVariableNames(m_fd,
00621 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00622 vntRet.get());
00623 break;
00624 case ID_ROBOT_EXECUTE:
00625 hr = bCap_RobotExecute(m_fd,
00626 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2),
00627 vntRet.get());
00628 break;
00629 case ID_ROBOT_ACCELERATE:
00630 hr = bCap_RobotAccelerate(m_fd,
00631 vntArgs.at(0).ulVal, vntArgs.at(1).lVal, vntArgs.at(2).fltVal, vntArgs.at(3).fltVal);
00632 break;
00633 case ID_ROBOT_CHANGE:
00634 hr = bCap_RobotChange(m_fd,
00635 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00636 break;
00637 case ID_ROBOT_CHUCK:
00638 hr = bCap_RobotChuck(m_fd,
00639 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00640 break;
00641 case ID_ROBOT_DRIVE:
00642 hr = bCap_RobotDrive(m_fd,
00643 vntArgs.at(0).ulVal, vntArgs.at(1).lVal, vntArgs.at(2).fltVal, vntArgs.at(3).bstrVal);
00644 break;
00645 case ID_ROBOT_GOHOME:
00646 hr = bCap_RobotGoHome(m_fd,
00647 vntArgs.at(0).ulVal);
00648 break;
00649 case ID_ROBOT_HALT:
00650 hr = bCap_RobotHalt(m_fd,
00651 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00652 break;
00653 case ID_ROBOT_HOLD:
00654 hr = bCap_RobotHold(m_fd,
00655 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00656 break;
00657 case ID_ROBOT_MOVE:
00658 hr = bCap_RobotMove(m_fd,
00659 vntArgs.at(0).ulVal, vntArgs.at(1).lVal, vntArgs.at(2), vntArgs.at(3).bstrVal);
00660 break;
00661 case ID_ROBOT_ROTATE:
00662 hr = bCap_RobotRotate(m_fd,
00663 vntArgs.at(0).ulVal, vntArgs.at(1), vntArgs.at(2).fltVal, vntArgs.at(3), vntArgs.at(4).bstrVal);
00664 break;
00665 case ID_ROBOT_SPEED:
00666 hr = bCap_RobotSpeed(m_fd,
00667 vntArgs.at(0).ulVal, vntArgs.at(1).lVal, vntArgs.at(2).fltVal);
00668 break;
00669 case ID_ROBOT_UNCHUCK:
00670 hr = bCap_RobotUnchuck(m_fd,
00671 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00672 break;
00673 case ID_ROBOT_UNHOLD:
00674 hr = bCap_RobotUnhold(m_fd,
00675 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00676 break;
00677 case ID_ROBOT_GETATTRIBUTE:
00678 vntRet->vt = VT_I4;
00679 hr = bCap_RobotGetAttribute(m_fd,
00680 vntArgs.at(0).ulVal,
00681 &vntRet->lVal);
00682 break;
00683 case ID_ROBOT_GETHELP:
00684 vntRet->vt = VT_BSTR;
00685 hr = bCap_RobotGetHelp(m_fd,
00686 vntArgs.at(0).ulVal,
00687 &vntRet->bstrVal);
00688 break;
00689 case ID_ROBOT_GETNAME:
00690 vntRet->vt = VT_BSTR;
00691 hr = bCap_RobotGetName(m_fd,
00692 vntArgs.at(0).ulVal,
00693 &vntRet->bstrVal);
00694 break;
00695 case ID_ROBOT_GETTAG:
00696 hr = bCap_RobotGetTag(m_fd,
00697 vntArgs.at(0).ulVal,
00698 vntRet.get());
00699 break;
00700 case ID_ROBOT_PUTTAG:
00701 hr = bCap_RobotPutTag(m_fd,
00702 vntArgs.at(0).ulVal, vntArgs.at(1));
00703 break;
00704 case ID_ROBOT_GETID:
00705 hr = bCap_RobotGetID(m_fd,
00706 vntArgs.at(0).ulVal,
00707 vntRet.get());
00708 break;
00709 case ID_ROBOT_PUTID:
00710 hr = bCap_RobotPutID(m_fd,
00711 vntArgs.at(0).ulVal, vntArgs.at(1));
00712 break;
00713 case ID_ROBOT_RELEASE:
00714 tmpKH.second = vntArgs.at(0).ulVal;
00715 hr = bCap_RobotRelease(m_fd,
00716 &vntArgs.at(0).ulVal);
00717 break;
00718
00719 case ID_TASK_GETVARIABLE:
00720 vntRet->vt = VT_UI4;
00721 hr = bCap_TaskGetVariable(m_fd,
00722 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2).bstrVal,
00723 &vntRet->ulVal);
00724 tmpKH.first = ID_VARIABLE_RELEASE;
00725 break;
00726 case ID_TASK_GETVARIABLENAMES:
00727 hr = bCap_TaskGetVariableNames(m_fd,
00728 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal,
00729 vntRet.get());
00730 break;
00731 case ID_TASK_EXECUTE:
00732 hr = bCap_TaskExecute(m_fd,
00733 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal, vntArgs.at(2),
00734 vntRet.get());
00735 break;
00736 case ID_TASK_START:
00737 hr = bCap_TaskStart(m_fd,
00738 vntArgs.at(0).ulVal, vntArgs.at(1).lVal, vntArgs.at(2).bstrVal);
00739 break;
00740 case ID_TASK_STOP:
00741 hr = bCap_TaskStop(m_fd,
00742 vntArgs.at(0).ulVal, vntArgs.at(1).lVal, vntArgs.at(2).bstrVal);
00743 break;
00744 case ID_TASK_DELETE:
00745 hr = bCap_TaskDelete(m_fd,
00746 vntArgs.at(0).ulVal, vntArgs.at(1).bstrVal);
00747 break;
00748 case ID_TASK_GETFILENAME:
00749 vntRet->vt = VT_BSTR;
00750 hr = bCap_TaskGetFileName(m_fd,
00751 vntArgs.at(0).ulVal,
00752 &vntRet->bstrVal);
00753 break;
00754 case ID_TASK_GETATTRIBUTE:
00755 vntRet->vt = VT_I4;
00756 hr = bCap_TaskGetAttribute(m_fd,
00757 vntArgs.at(0).ulVal,
00758 &vntRet->lVal);
00759 break;
00760 case ID_TASK_GETHELP:
00761 vntRet->vt = VT_BSTR;
00762 hr = bCap_TaskGetHelp(m_fd,
00763 vntArgs.at(0).ulVal,
00764 &vntRet->bstrVal);
00765 break;
00766 case ID_TASK_GETNAME:
00767 vntRet->vt = VT_BSTR;
00768 hr = bCap_TaskGetName(m_fd,
00769 vntArgs.at(0).ulVal,
00770 &vntRet->bstrVal);
00771 break;
00772 case ID_TASK_GETTAG:
00773 hr = bCap_TaskGetTag(m_fd,
00774 vntArgs.at(0).ulVal,
00775 vntRet.get());
00776 break;
00777 case ID_TASK_PUTTAG:
00778 hr = bCap_TaskPutTag(m_fd,
00779 vntArgs.at(0).ulVal, vntArgs.at(1));
00780 break;
00781 case ID_TASK_GETID:
00782 hr = bCap_TaskGetID(m_fd,
00783 vntArgs.at(0).ulVal,
00784 vntRet.get());
00785 break;
00786 case ID_TASK_PUTID:
00787 hr = bCap_TaskPutID(m_fd,
00788 vntArgs.at(0).ulVal, vntArgs.at(1));
00789 break;
00790 case ID_TASK_RELEASE:
00791 tmpKH.second = vntArgs.at(0).ulVal;
00792 hr = bCap_TaskRelease(m_fd,
00793 &vntArgs.at(0).ulVal);
00794 break;
00795
00796 case ID_VARIABLE_GETDATETIME:
00797 hr = bCap_VariableGetDateTime(m_fd,
00798 vntArgs.at(0).ulVal,
00799 vntRet.get());
00800 break;
00801 case ID_VARIABLE_GETVALUE:
00802 hr = bCap_VariableGetValue(m_fd,
00803 vntArgs.at(0).ulVal,
00804 vntRet.get());
00805 break;
00806 case ID_VARIABLE_PUTVALUE:
00807 hr = bCap_VariablePutValue(m_fd,
00808 vntArgs.at(0).ulVal, vntArgs.at(1));
00809 break;
00810 case ID_VARIABLE_GETATTRIBUTE:
00811 vntRet->vt = VT_I4;
00812 hr = bCap_VariableGetAttribute(m_fd,
00813 vntArgs.at(0).ulVal,
00814 &vntRet->lVal);
00815 break;
00816 case ID_VARIABLE_GETHELP:
00817 vntRet->vt = VT_BSTR;
00818 hr = bCap_VariableGetHelp(m_fd,
00819 vntArgs.at(0).ulVal,
00820 &vntRet->bstrVal);
00821 break;
00822 case ID_VARIABLE_GETNAME:
00823 vntRet->vt = VT_BSTR;
00824 hr = bCap_VariableGetName(m_fd,
00825 vntArgs.at(0).ulVal,
00826 &vntRet->bstrVal);
00827 break;
00828 case ID_VARIABLE_GETTAG:
00829 hr = bCap_VariableGetTag(m_fd,
00830 vntArgs.at(0).ulVal,
00831 vntRet.get());
00832 break;
00833 case ID_VARIABLE_PUTTAG:
00834 hr = bCap_VariablePutTag(m_fd,
00835 vntArgs.at(0).ulVal, vntArgs.at(1));
00836 break;
00837 case ID_VARIABLE_GETID:
00838 hr = bCap_VariableGetID(m_fd,
00839 vntArgs.at(0).ulVal,
00840 vntRet.get());
00841 break;
00842 case ID_VARIABLE_PUTID:
00843 hr = bCap_VariablePutID(m_fd,
00844 vntArgs.at(0).ulVal, vntArgs.at(1));
00845 break;
00846 case ID_VARIABLE_GETMICROSECOND:
00847 vntRet->vt = VT_I4;
00848 hr = bCap_VariableGetMicrosecond(m_fd,
00849 vntArgs.at(0).ulVal,
00850 &vntRet->lVal);
00851 break;
00852 case ID_VARIABLE_RELEASE:
00853 tmpKH.second = vntArgs.at(0).ulVal;
00854 hr = bCap_VariableRelease(m_fd,
00855 &vntArgs.at(0).ulVal);
00856 break;
00857
00858 case ID_COMMAND_EXECUTE:
00859 hr = bCap_CommandExecute(m_fd,
00860 vntArgs.at(0).ulVal, vntArgs.at(1).lVal,
00861 vntRet.get());
00862 break;
00863 case ID_COMMAND_CANCEL:
00864 hr = bCap_CommandCancel(m_fd,
00865 vntArgs.at(0).ulVal);
00866 break;
00867 case ID_COMMAND_GETTIMEOUT:
00868 vntRet->vt = VT_I4;
00869 hr = bCap_CommandGetTimeout(m_fd,
00870 vntArgs.at(0).ulVal,
00871 &vntRet->lVal);
00872 break;
00873 case ID_COMMAND_PUTTIMEOUT:
00874 hr = bCap_CommandPutTimeout(m_fd,
00875 vntArgs.at(0).ulVal, vntArgs.at(1).lVal);
00876 break;
00877 case ID_COMMAND_GETSTATE:
00878 vntRet->vt = VT_I4;
00879 hr = bCap_CommandGetState(m_fd,
00880 vntArgs.at(0).ulVal,
00881 &vntRet->lVal);
00882 break;
00883 case ID_COMMAND_GETPARAMETERS:
00884 hr = bCap_CommandGetParameters(m_fd,
00885 vntArgs.at(0).ulVal,
00886 vntRet.get());
00887 break;
00888 case ID_COMMAND_PUTPARAMETERS:
00889 hr = bCap_CommandPutParameters(m_fd,
00890 vntArgs.at(0).ulVal, vntArgs.at(1));
00891 break;
00892 case ID_COMMAND_GETRESULT:
00893 hr = bCap_CommandGetResult(m_fd,
00894 vntArgs.at(0).ulVal,
00895 vntRet.get());
00896 break;
00897 case ID_COMMAND_GETATTRIBUTE:
00898 vntRet->vt = VT_I4;
00899 hr = bCap_CommandGetAttribute(m_fd,
00900 vntArgs.at(0).ulVal,
00901 &vntRet->lVal);
00902 break;
00903 case ID_COMMAND_GETHELP:
00904 vntRet->vt = VT_BSTR;
00905 hr = bCap_CommandGetHelp(m_fd,
00906 vntArgs.at(0).ulVal,
00907 &vntRet->bstrVal);
00908 break;
00909 case ID_COMMAND_GETNAME:
00910 vntRet->vt = VT_BSTR;
00911 hr = bCap_CommandGetName(m_fd,
00912 vntArgs.at(0).ulVal,
00913 &vntRet->bstrVal);
00914 break;
00915 case ID_COMMAND_GETTAG:
00916 hr = bCap_CommandGetTag(m_fd,
00917 vntArgs.at(0).ulVal,
00918 vntRet.get());
00919 break;
00920 case ID_COMMAND_PUTTAG:
00921 hr = bCap_CommandPutTag(m_fd,
00922 vntArgs.at(0).ulVal, vntArgs.at(1));
00923 break;
00924 case ID_COMMAND_GETID:
00925 hr = bCap_CommandGetID(m_fd,
00926 vntArgs.at(0).ulVal,
00927 vntRet.get());
00928 break;
00929 case ID_COMMAND_PUTID:
00930 hr = bCap_CommandPutID(m_fd,
00931 vntArgs.at(0).ulVal, vntArgs.at(1));
00932 break;
00933 case ID_COMMAND_RELEASE:
00934 tmpKH.second = vntArgs.at(0).ulVal;
00935 hr = bCap_CommandRelease(m_fd,
00936 &vntArgs.at(0).ulVal);
00937 break;
00938 case ID_MESSAGE_REPLY:
00939 hr = bCap_MessageReply(m_fd,
00940 vntArgs.at(0).ulVal, vntArgs.at(1));
00941 break;
00942 case ID_MESSAGE_CLEAR:
00943 hr = bCap_MessageClear(m_fd,
00944 vntArgs.at(0).ulVal);
00945 break;
00946 case ID_MESSAGE_GETDATETIME:
00947 hr = bCap_MessageGetDateTime(m_fd,
00948 vntArgs.at(0).ulVal,
00949 vntRet.get());
00950 break;
00951 case ID_MESSAGE_GETDESCRIPTION:
00952 vntRet->vt = VT_BSTR;
00953 hr = bCap_MessageGetDescription(m_fd,
00954 vntArgs.at(0).ulVal,
00955 &vntRet->bstrVal);
00956 break;
00957 case ID_MESSAGE_GETDESTINATION:
00958 vntRet->vt = VT_BSTR;
00959 hr = bCap_MessageGetDestination(m_fd,
00960 vntArgs.at(0).ulVal,
00961 &vntRet->bstrVal);
00962 break;
00963 case ID_MESSAGE_GETNUMBER:
00964 vntRet->vt = VT_I4;
00965 hr = bCap_MessageGetNumber(m_fd,
00966 vntArgs.at(0).ulVal,
00967 &vntRet->lVal);
00968 break;
00969 case ID_MESSAGE_GETSERIALNUMBER:
00970 vntRet->vt = VT_I4;
00971 hr = bCap_MessageGetSerialNumber(m_fd,
00972 vntArgs.at(0).ulVal,
00973 &vntRet->lVal);
00974 break;
00975 case ID_MESSAGE_GETSOURCE:
00976 vntRet->vt = VT_BSTR;
00977 hr = bCap_MessageGetSource(m_fd,
00978 vntArgs.at(0).ulVal,
00979 &vntRet->bstrVal);
00980 break;
00981 case ID_MESSAGE_GETVALUE:
00982 hr = bCap_MessageGetValue(m_fd,
00983 vntArgs.at(0).ulVal,
00984 vntRet.get());
00985 break;
00986 case ID_MESSAGE_RELEASE:
00987 tmpKH.second = vntArgs.at(0).ulVal;
00988 hr = bCap_MessageRelease(m_fd,
00989 &vntArgs.at(0).ulVal);
00990 break;
00991 }
00992 } catch(std::out_of_range&) {
00993 hr = DISP_E_BADINDEX;
00994 }
00995
00996 if(hr == S_OK) {
00997 if(tmpKH.first > 0) {
00998 tmpKH.second = vntRet->ulVal;
00999 m_vecKH.push_back(tmpKH);
01000 }
01001 else if(tmpKH.second > 0) {
01002 KeyHandle_Vec::iterator it;
01003 for(it = m_vecKH.begin(); it != m_vecKH.end(); it++) {
01004 if((func_id == it->first)
01005 && (tmpKH.second == it->second))
01006 {
01007 m_vecKH.erase(it);
01008 break;
01009 }
01010 }
01011 }
01012 }
01013
01014 return hr;
01015 }
01016
01017 }
01018