// 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;
}
