// Call back Declaration ReturnType TouchSensorComp::onInitialize() { // XML에 저장된 프라퍼티를 parameter에 저장 Property parameter; std::map<std::string, std::string> temp = getPropertyMap(); parameter.SetProperty(temp); // dll 파일이름을 확인하여 없으면 에러 리턴 if(parameter.FindName("APIName") == false) { PrintMessage("ERROR : TouchSensorComp::onInitialize() -> Can't find the APIName in property\n"); return OPROS_FIND_PROPERTY_ERROR; } #if defined(WIN32) // DLL 로드 hOprosAPI = LoadLibrary((LPCSTR)parameter.GetValue("DllName").c_str()); if(hOprosAPI == NULL) { PrintMessage("ERROR : TouchSensorComp::onInitialize() -> Can't find the %s\n", parameter.GetValue("DllName").c_str()); return OPROS_FIND_DLL_ERROR; } // API 로드 GET_OPROS_API getOprosAPI; getOprosAPI = (GET_OPROS_API)GetProcAddress(hOprosAPI, "GetAPI"); if(getOprosAPI == NULL) { PrintMessage("ERROR : TouchSensorComp::onInitialize() -> Can't get a handle of GetAPI Funtion\n"); FreeLibrary(hOprosAPI); hOprosAPI = NULL; return OPROS_LOAD_DLL_ERROR; } #else hOprosAPI = dlopen(parameter.GetValue("DllName").c_str(), RTLD_LAZY); if(hOprosAPI == NULL) { PrintMessage("ERROR : TouchSensorComp::onInitialize() -> Can't find the %s\n", parameter.GetValue("APIName").c_str()); return OPROS_FIND_DLL_ERROR; } GET_OPROS_API getOprosAPI; getOprosAPI = (GET_OPROS_API)dlsym(hOprosAPI, "GetAPI"); char *error = dlerror(); if(error != NULL) { PrintMessage("ERROR : TouchSensorComp::onInitialize() -> Can't get a handle of GetAPI Funtion\n"); dlclose(hOprosAPI); hOprosAPI = NULL; return OPROS_LOAD_DLL_ERROR; } #endif touchSensor = dynamic_cast<TouchSensor *>(getOprosAPI()); if(touchSensor == NULL) { PrintMessage("ERROR : TouchSensorComp::onInitialize() -> Can't get a handle of TouchSensor API\n"); #if defined(WIN32) FreeLibrary(hOprosAPI); #else dlclose(hOprosAPI); #endif hOprosAPI = NULL; return OPROS_LOAD_DLL_ERROR; } // API 초기화 if(touchSensor->Initialize(parameter) != API_SUCCESS) { PrintMessage("ERROR : TouchSensorComp::onInitialize() -> Can't intilaize a TouchSensor API\n"); delete touchSensor; touchSensor = NULL; #if defined(WIN32) FreeLibrary(hOprosAPI); #else dlclose(hOprosAPI); #endif hOprosAPI = NULL; return OPROS_INITIALIZE_API_ERROR; } error = 0; return OPROS_SUCCESS; }
bool KITECH_InverseDynamicsControlComp::SetParameter(Property parameter) { int jointCount; int jointType, jointAxis; double x, y, z, phi, theta, psi; double mass, cx, cy, cz; double ixx, iyy, izz, ixy, iyz, izx; char name[128]; double gravity[3] = {0., 0., -9.81 }; onDestroy(); // Setup a control period if(parameter.FindName("Period") == false) { PrintMessage("ERROR : KITECH_InverseDynamicsControlComp::SetParameter() -> Can't find the Period in property\n"); return false; } _dynamics = new InverseDynamicsControl(gravity, atof(parameter.GetValue("Period").c_str())); // Setup a count of joints if(parameter.FindName("JointCount") == false) { PrintMessage("ERROR : KITECH_InverseDynamicsControlComp::SetParameter() -> Can't find the JointCount in property\n"); goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; } jointCount = atoi(parameter.GetValue("JointCount").c_str()); // Set gains if(parameter.FindName("Kp") == false) { PrintMessage("ERROR : KITECH_InverseDynamicsControlComp::SetParameter() -> Can't find the JointCount in property\n"); goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; } _dynamics->SetKp(atof(parameter.GetValue("Kp").c_str())); if(parameter.FindName("Ki") == false) { PrintMessage("ERROR : KITECH_InverseDynamicsControlComp::SetParameter() -> Can't find the JointCount in property\n"); goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; } _dynamics->SetKi(atof(parameter.GetValue("Ki").c_str())); if(parameter.FindName("Kd") == false) { PrintMessage("ERROR : KITECH_InverseDynamicsControlComp::SetParameter() -> Can't find the JointCount in property\n"); goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; } _dynamics->SetKd(atof(parameter.GetValue("Kd").c_str())); // Setup a joint information PrintMessage("KITECH_InverseDynamicsControlComp Joint Information\n"); for(int i = 0; i < jointCount; i++) { sprintf(name, "JointType%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else jointType = atoi(parameter.GetValue(name).c_str()); sprintf(name, "JointAxis%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else jointAxis = atoi(parameter.GetValue(name).c_str()); sprintf(name, "X%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else x = atof(parameter.GetValue(name).c_str()); sprintf(name, "Y%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else y = atof(parameter.GetValue(name).c_str()); sprintf(name, "Z%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else z = atof(parameter.GetValue(name).c_str()); sprintf(name, "Phi%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else phi = atof(parameter.GetValue(name).c_str()); sprintf(name, "Theta%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else theta = atof(parameter.GetValue(name).c_str()); sprintf(name, "Psi%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else psi = atof(parameter.GetValue(name).c_str()); sprintf(name, "Mass%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else mass = atof(parameter.GetValue(name).c_str()); sprintf(name, "CX%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else cx = atof(parameter.GetValue(name).c_str()); sprintf(name, "CY%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else cy = atof(parameter.GetValue(name).c_str()); sprintf(name, "CZ%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else cz = atof(parameter.GetValue(name).c_str()); sprintf(name, "IXX%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else ixx = atof(parameter.GetValue(name).c_str()); sprintf(name, "IYY%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else iyy = atof(parameter.GetValue(name).c_str()); sprintf(name, "IZZ%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else izz = atof(parameter.GetValue(name).c_str()); sprintf(name, "IXY%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else ixy = atof(parameter.GetValue(name).c_str()); sprintf(name, "IYZ%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else iyz = atof(parameter.GetValue(name).c_str()); sprintf(name, "IZX%d", i); if(parameter.FindName(name) == false) goto ERROR_KITECH_InverseDynamicsControlComp_SetParameter; else izx = atof(parameter.GetValue(name).c_str()); _dynamics->AttachJoint(jointType, jointAxis, mass, cx, cy, cz, ixx, iyy, izz, ixy, iyz, izx, x, y, z, phi, theta, psi, 0.3); PrintMessage("Axis%d %d %d %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", i, jointType, jointAxis, x, y, z, phi, theta, psi, mass, cx, cy, cz, ixx, iyy, izz, ixy, iyz, izx); } this->parameter = parameter; return true; ERROR_KITECH_InverseDynamicsControlComp_SetParameter : PrintMessage("ERROR : KITECH_InverseDynamicsControlComp::SetParameter() -> Can't find parameters in property\n"); onDestroy(); return false; }
// Call back Declaration ReturnType InertiaMeasurementUnitComp::onInitialize() { Property parameter; std::map<std::string, std::string> temp = getPropertyMap(); parameter.SetProperty(temp); if(parameter.FindName("ApiName") == false) { PrintMessage("ERROR : InertiaMeasurementUnitComp::onInitialize() -> Can't find the APIName in property\n"); return lastError = OPROS_FIND_PROPERTY_ERROR; } #if defined(WIN32) // DLL 로드 hOprosAPI = LoadLibrary((LPCSTR)parameter.GetValue("ApiName").c_str()); if(hOprosAPI == NULL) { PrintMessage("ERROR : InertiaMeasurementUnitComp::onInitialize() -> Can't find the %s\n", parameter.GetValue("ApiName").c_str()); return lastError = OPROS_FIND_DLL_ERROR; } // API 로드 GET_OPROS_API getOprosAPI; getOprosAPI = (GET_OPROS_API)GetProcAddress(hOprosAPI, "GetAPI"); if(getOprosAPI == NULL) { PrintMessage("ERROR : InertiaMeasurementUnitComp::onInitialize() -> Can't get a handle of GetAPI Funtion\n"); FreeLibrary(hOprosAPI); hOprosAPI = NULL; return lastError = OPROS_LOAD_DLL_ERROR; } // API Casting imu = dynamic_cast<InertiaMeasurementUnit *>(getOprosAPI()); if(imu == NULL) { PrintMessage("ERROR : InertiaMeasurementUnitComp::onInitialize() -> Can't get a handle of AccelerationSensor API\n"); FreeLibrary(hOprosAPI); hOprosAPI = NULL; return lastError = OPROS_LOAD_DLL_ERROR; } #else // Shared Library 로드 hOprosAPI = dlopen(parameter.GetValue("ApiName").c_str(), RTLD_NOW); if(hOprosAPI == NULL) { PrintMessage("ERROR : InertiaMeasurementUnitComp::onInitialize() -> Can't find the %s\n", parameter.GetValue("ApiName").c_str()); return lastError = OPROS_FIND_DLL_ERROR; } // API 로드 GET_OPROS_API getOprosAPI = (GET_OPROS_API)dlsym(hOprosAPI, "GetAPI"); char *error = dlerror(); if(error != NULL) { PrintMessage("ERROR : InertiaMeasurementUnitComp::onInitialize() -> Can't get a handle of GetAPI Funtion\n"); dlclose(hOprosAPI); hOprosAPI = NULL; return lastError = OPROS_LOAD_DLL_ERROR; } // API Casting imu = static_cast<InertiaMeasurementUnit *>(getOprosAPI()); if(imu == NULL) { PrintMessage("ERROR : InertiaMeasurementUnitComp::onInitialize() -> Can't get a handle of AccelerationSensor API\n"); dlclose(hOprosAPI); hOprosAPI = NULL; return lastError = OPROS_LOAD_DLL_ERROR; } #endif if(imu->Initialize(parameter) != API_SUCCESS) { delete imu; imu = NULL; #if defined(WIN32) FreeLibrary(hOprosAPI); #else dlclose(hOprosAPI); #endif hOprosAPI = NULL; return lastError = OPROS_INITIALIZE_API_ERROR; } return lastError = OPROS_SUCCESS; }
int VidereSthDcsgVarMultiCamera::SetParameter(Property parameter) { if (!parameter.FindName("Count") ) { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter()->Can't find parameter(s) name.\n"); return API_ERROR; } _nCameraCount = atoi( (parameter.GetValue("Count")).c_str() ); if( 0 < _nCameraCount ) { if( _id ) { free(_id); _id = 0; } if( _width ) { free(_width); _width = 0; } if( _height ) { free(_height); _height = 0; } if( _pixelByte ) { free(_pixelByte); _pixelByte = 0; } if( _flip ) { free(_flip); _flip = 0; } _id = (int *)malloc(_nCameraCount*sizeof(int)); _width = (int *)malloc(_nCameraCount*sizeof(int)); _height = (int *)malloc(_nCameraCount*sizeof(int)); _pixelByte = (int *)malloc(_nCameraCount*sizeof(int)); _flip = (int *)malloc(_nCameraCount*sizeof(int)); // CalibratedParameter parameter를 확인하여 없으면 에러 리턴 if(parameter.FindName("CalibrationFilename") == false) { PrintMessage("WARNING : VidereSthDcsgVarMultiCamera::SetParameter() -> Can't find the Calibration file in property\n"); //return API_ERROR; _calibrationFilename = parameter.GetValue("APIName"); // apiname으로 대체 } else _calibrationFilename = parameter.GetValue("CalibrationFilename"); char str[256]; for( int i = 0 ; i < _nCameraCount ; i++ ) { sprintf(str, "CameraID%d", i); if( !parameter.FindName(str) ) { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter()->Can't find parameter(s) name %s\n", str); return API_ERROR; } _id[i] = atoi( (parameter.GetValue(string(str))).c_str() ); sprintf(str, "Width%d", i); if( !parameter.FindName(str) ) { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter()->Can't find parameter(s) name %s\n", str); return API_ERROR; } _width[i] = atoi( (parameter.GetValue(string(str))).c_str() ); sprintf(str, "Height%d", i); if( !parameter.FindName(str) ) { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter()->Can't find parameter(s) name %s\n", str); return API_ERROR; } _height[i] = atoi( (parameter.GetValue(string(str))).c_str() ); sprintf(str, "PixelBytes%d", i); if( !parameter.FindName(str) ) { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter()->Can't find parameter(s) name %s\n", str); return API_ERROR; } _pixelByte[i] = atoi( (parameter.GetValue(string(str))).c_str() ); sprintf(str, "Flip%d", i); if( !parameter.FindName(str) ) { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter()->Can't find parameter(s) name %s\n", str); return API_ERROR; } _flip[i] = atoi( (parameter.GetValue(string(str))).c_str() ); // check image size if(i>0) { if(_width[0]!=_width[i] || _height[0]!=_height[i] || _pixelByte[0]!=_pixelByte[i]) { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter() -> Each camera must have same parameters(width,height,pixelbytes)!\n"); return API_ERROR; } } } PrintMessage("SUCCESS: VidereSthDcsgVarMultiCamera::SetParameter() OK!\n"); PrintMessage("Count:%d, calibrationFilename:%s\n", _nCameraCount, _calibrationFilename.c_str() ); for(int i=0; i<_nCameraCount;i++) PrintMessage("CameraID:%d, width:%d, height:%d, pixelbytes:%d, flip:%d\n", _id[i],_width[i],_height[i],_pixelByte[i],_flip[i]); this->parameter = parameter; return API_SUCCESS; } else { PrintMessage("Error:VidereSthDcsgVarMultiCamera::SetParameter()->Camera count is 0.\n"); return API_ERROR; } }
int Robotis_DynamixelUART::InitializeUART(Property parameter) { Property uartParameter; char *error = NULL; if(parameter.FindName("UARTAPIName") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; if(parameter.FindName("PortName") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; else uartParameter.SetValue("PortName", parameter.GetValue("PortName")); if(parameter.FindName("TimeOut") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; else uartParameter.SetValue("TimeOut", parameter.GetValue("TimeOut")); if(parameter.FindName("BaudRate") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; else uartParameter.SetValue("BaudRate", parameter.GetValue("BaudRate")); if(parameter.FindName("DataBits") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; else uartParameter.SetValue("DataBits", parameter.GetValue("DataBits")); if(parameter.FindName("StopBits") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; else uartParameter.SetValue("StopBits", parameter.GetValue("StopBits")); if(parameter.FindName("Parity") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; else uartParameter.SetValue("Parity", parameter.GetValue("Parity")); if(parameter.FindName("FlowControl") == false) goto ERROR_Robotis_DynamixelUART_InitializeUART; else uartParameter.SetValue("FlowControl", parameter.GetValue("FlowControl")); if(uart != NULL) { delete uart; uart = NULL; } #if defined(WIN32) // DLL 로드 hOprosAPI = LoadLibrary((LPCSTR)parameter.GetValue("UARTAPIName").c_str()); if(hOprosAPI == NULL) { return API_ERROR; } GET_OPROS_API getOprosAPI; getOprosAPI = (GET_OPROS_API)GetProcAddress(hOprosAPI, "GetAPI"); if(getOprosAPI == NULL) { FreeLibrary(hOprosAPI); hOprosAPI = NULL; return API_ERROR; } #else hOprosAPI = dlopen(parameter.GetValue("UARTAPIName").c_str(), RTLD_LAZY); if(hOprosAPI == NULL) { return API_ERROR; } GET_OPROS_API getOprosAPI; getOprosAPI = (GET_OPROS_API)dlsym(hOprosAPI, "GetAPI"); error = dlerror(); if(error != NULL) { dlclose(hOprosAPI); hOprosAPI = NULL; return API_ERROR; } #endif uart = (UART *)getOprosAPI(); if(uart == NULL) { Finalize(); return API_ERROR; } // API 초기화 if(uart->Initialize(uartParameter) < 0) { Finalize(); return API_ERROR; } if(uart->Enable() < 0) { Finalize(); return API_ERROR; } return API_SUCCESS; ERROR_Robotis_DynamixelUART_InitializeUART : PrintMessage("ERROR : Robotis_DynamixelUART::InitializeUART() -> Can't find a property in parameter.\n"); return API_ERROR; }