bool QFileStream::_OpenPlatformImplementation(const QPath &filePath, const EQFileOpenMode &eOpenMode, const bool bIsWritingAllowed, QFileStream::NativeHandle &handle, EQFileSystemError &eErrorInfo) { bool bSuccess = true; using Kinesis::QuimeraEngine::Common::DataTypes::QArrayResult; using Kinesis::QuimeraEngine::Common::DataTypes::i8_q; using Kinesis::QuimeraEngine::Common::DataTypes::EQTextEncoding; QArrayResult<i8_q> szPath = filePath.ToString().ToBytes(EQTextEncoding::E_UTF8); int nAccess = bIsWritingAllowed ? O_RDWR: O_RDONLY; int nPermissions = S_IRWXU; // Read, write and execution permissions for the owner int nOpenMode = 0; switch(eOpenMode) { case EQFileOpenMode::E_Append: case EQFileOpenMode::E_Open: nOpenMode = 0; break; case EQFileOpenMode::E_Create: nOpenMode = O_CREAT | O_EXCL; break; case EQFileOpenMode::E_CreateOrOverwrite: nOpenMode = O_CREAT | O_TRUNC; break; case EQFileOpenMode::E_OpenOrCreate: nOpenMode = O_CREAT; break; default: break; } handle = open(szPath.Get(), nAccess | nOpenMode, nPermissions); const bool FILE_OPENED_SUCCESSFULLY = handle >= 0; if(!FILE_OPENED_SUCCESSFULLY) { bSuccess = false; error_t lastError = errno; QE_ASSERT_ERROR(handle >= 0, string_q("An unexpected error occurred when opening the file \"") + filePath.GetAbsolutePath() + "\". The error code was: " + string_q::FromInteger(lastError) + "."); if(lastError == EACCES) eErrorInfo = EQFileSystemError::E_NoPermissions; else eErrorInfo = EQFileSystemError::E_Unknown; } return bSuccess; }
bool QFileStream::_OpenPlatformImplementation(const QPath &filePath, const EQFileOpenMode &eOpenMode, const bool bIsWritingAllowed, QFileStream::NativeHandle &handle, EQFileSystemError &eErrorInfo) { bool bSuccess = true; using Kinesis::QuimeraEngine::Common::DataTypes::QArrayResult; using Kinesis::QuimeraEngine::Common::DataTypes::i8_q; QArrayResult<i8_q> arPath = filePath.ToString().ToBytes(string_q::GetLocalEncodingUTF16()); const wchar_t* szPath = rcast_q(arPath.Get(), wchar_t*); DWORD uAccess = bIsWritingAllowed ? GENERIC_READ | GENERIC_WRITE : GENERIC_READ; DWORD uOpenMode = 0; switch(eOpenMode) { case EQFileOpenMode::E_Append: case EQFileOpenMode::E_Open: uOpenMode = OPEN_EXISTING; break; case EQFileOpenMode::E_Create: uOpenMode = CREATE_NEW; break; case EQFileOpenMode::E_CreateOrOverwrite: uOpenMode = CREATE_ALWAYS; break; case EQFileOpenMode::E_OpenOrCreate: uOpenMode = OPEN_ALWAYS; break; default: break; } handle = ::CreateFileW(szPath, uAccess, FILE_SHARE_READ, NULL, uOpenMode, FILE_FLAG_RANDOM_ACCESS, NULL); const bool FILE_OPENED_SUCCESSFULLY = handle != INVALID_HANDLE_VALUE; if(!FILE_OPENED_SUCCESSFULLY) { bSuccess = false; DWORD uCreateFileWLastError = ::GetLastError(); QE_ASSERT_ERROR(handle != INVALID_HANDLE_VALUE, string_q("An unexpected error occurred when opening the file \"") + filePath.GetAbsolutePath() + "\". The error code was: " + string_q::FromInteger(uCreateFileWLastError) + "."); if(uCreateFileWLastError == ERROR_ACCESS_DENIED) eErrorInfo = EQFileSystemError::E_NoPermissions; else eErrorInfo = EQFileSystemError::E_Unknown; } return bSuccess; }
int main(int argc, char** argv) { const string wcFile = "Kr16WallWorkCell/Scene.wc.xml"; const string deviceName = "KukaKr16"; cout << "Trying to use workcell " << wcFile << " and device " << deviceName << endl; WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile); Device::Ptr device = wc->findDevice(deviceName); if (device == NULL) { cerr << "Device: " << deviceName << " not found!" << endl; return 0; } const char* stats_str; double extend; if (argc > 1) { stats_str = argv[1]; extend = atof(argv[2]); }else { stats_str = "stat.txt"; extend = 0.1; } cout << extend << endl << stats_str <<endl; Math::seed(); State state = wc->getDefaultState(); Q start(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573); device->setQ(start,state); MovableFrame* bottle = wc->findFrame<MovableFrame>("Bottle"); Kinematics::gripFrame(bottle,device->getEnd(),state); CollisionDetector detector(wc, ProximityStrategyFactory::makeDefaultCollisionStrategy()); PlannerConstraint constraint = PlannerConstraint::make(&detector,device,state); /** Most easy way: uses default parameters based on given device sampler: QSampler::makeUniform(device) metric: PlannerUtil::normalizingInfinityMetric(device->getBounds()) extend: 0.05 */ //QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, device, RRTPlanner::RRTConnect); /** More complex way: allows more detailed definition of parameters and methods */ QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr()); QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>(); //double extend = 0.1; Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573); //Q to(6,1.7,0.6,-0.8,0.3,0.7,-0.5); // Very difficult for planner Q to(6,1.571,0.006,0.030,0.153,0.762,4.490); if (!checkCollisions(device, state, detector, from)) return 0; if (!checkCollisions(device, state, detector, to)) return 0; cout << "Planning from " << from << " to " << to << endl; ofstream stat; stat.open(stats_str); stat << "Path length,Time" << endl; QPath path; Timer t; while (extend <= 0.5) { QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect); t.resetAndResume(); planner->query(from,to,path,MAXTIME); t.pause(); stat << path.size() << "," << t.getTime() << "," << extend << endl; for (int x=1; x<100; x++) { QPath path2; t.resetAndResume(); planner->query(from,to,path2,MAXTIME); t.pause(); stat << path2.size() << "," << t.getTime() << "," << extend << endl; if (path2.size() < path.size()) { path = path2; } } extend += 0.01; } if (t.getTime() >= MAXTIME) { cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl; } ofstream LUA; //create file object LUA.open("LUA.txt"); //open the file //write the LUA "header" LUA << "wc = rws.getRobWorkStudio():getWorkCell()\n"; LUA << "state = wc:getDefaultState() \ndevice = wc:findDevice(\"KukaKr16\")\n"; LUA << "bottle = wc:findFrame(\"Bottle\")\n"; LUA << "function setQ(q)\n"; LUA << " qq = rw.Q(#q,q[1],q[2],q[3],q[4],q[5],q[6])\n"; LUA << " device:setQ(qq,state)\n"; LUA << " rws.getRobWorkStudio():setState(state)\n"; LUA << " rw.sleep(0.1)\n"; LUA << "end\n"; LUA << "setQ({-3.142, -0.827, -3.002, -3.143, 0.099, -1.573})\n"; LUA << "rw.gripFrame(bottle,device:getEnd(),state)\n"; for (QPath::iterator it = path.begin(); it < path.end(); it++) { //cout << *it << endl; //output path to Terminal ostringstream out; out << *it; //write path to ostring string out2 = out.str(); //convert to string LUA << "setQ(" << out2.substr(4,out2.length()) << ")" << endl; // write path to LUA file. } LUA.close(); //close file cout << "Program done." << endl; return 0; }
bool QFileStream::_ClosePlarformImplementation(const QFileStream::NativeHandle &handle, const QPath &filePath) { bool bSuccess = true; int nResult = close(handle); if(nResult < 0) { bSuccess = false; error_t lastError = errno; QE_ASSERT_ERROR(nResult >= 0, string_q("An unexpected error occurred when closing the file \"") + filePath.GetAbsolutePath() + "\". The error code was: " + string_q::FromInteger(lastError) + "."); } return bSuccess; }
bool QFileStream::_WritePlatformImplementation(const void* pInputBuffer, const pointer_uint_q uNumberOfBytes, const pointer_uint_q uFileOffset, const QFileStream::NativeHandle &handle, const QPath &filePath) { bool bSuccess = true; ssize_t nResult = pwrite(handle, pInputBuffer, uNumberOfBytes, uFileOffset); if(nResult < 0) { bSuccess = false; error_t lastError = errno; QE_ASSERT_ERROR(nResult >= 0, string_q("An unexpected error occurred when writing to the file \"") + filePath.GetAbsolutePath() + "\". The error code was: " + string_q::FromInteger(lastError) + "."); } return bSuccess; }
bool QFileStream::_ClosePlarformImplementation(const QFileStream::NativeHandle &handle, const QPath &filePath) { bool bSuccess = true; static const BOOL CLOSE_OPERATION_FAILED = 0; BOOL uCloseHandleResult = ::CloseHandle(handle); if(uCloseHandleResult == CLOSE_OPERATION_FAILED) { bSuccess = false; DWORD uCloseHandleLastError = ::GetLastError(); QE_ASSERT_ERROR(uCloseHandleResult != 0, string_q("An unexpected error occurred when closing the file \"") + filePath.GetAbsolutePath() + "\". The error code was: " + string_q::FromInteger(uCloseHandleLastError) + "."); } return bSuccess; }
bool QFileStream::_WritePlatformImplementation(const void* pInputBuffer, const pointer_uint_q uNumberOfBytes, const pointer_uint_q uFileOffset, const QFileStream::NativeHandle &handle, const QPath &filePath) { bool bSuccess = true; OVERLAPPED offsets; memset(&offsets, 0, sizeof(OVERLAPPED)); // The structure MUST be zero-initialized // Sets the offset from where to start reading #if QE_OS_WINDOWS == 32 offsets.Offset = uFileOffset; offsets.OffsetHigh = 0; #elif QE_OS_WINDOWS == 64 const u32_q* pPosition = rcast_q(&uFileOffset, const u32_q*); #if QE_ENDIANNESS == QE_ENDIANNESS_LITTLEENDIAN offsets.Offset = pPosition[1]; offsets.OffsetHigh = pPosition[0]; #elif QE_ENDIANNESS == QE_ENDIANNESS_BIGENDIAN offsets.Offset = pPosition[0]; offsets.OffsetHigh = pPosition[1]; #endif #endif static const BOOL WRITE_OPERATION_FAILED = 0; BOOL uWriteFileResult = ::WriteFile(handle, pInputBuffer, uNumberOfBytes, NULL, &offsets); if(uWriteFileResult == WRITE_OPERATION_FAILED) { bSuccess = false; DWORD uWriteFileLastError = ::GetLastError(); QE_ASSERT_WARNING(uWriteFileResult != WRITE_OPERATION_FAILED, string_q("An unexpected error occurred when writing to the file \"") + filePath.GetAbsolutePath() + "\". The error code was: " + string_q::FromInteger(uWriteFileLastError) + "."); } return bSuccess; }
EQFileSystemError QFileStream::Open(const QPath &filePath, const EQFileOpenMode &eOpenMode) { QE_ASSERT_ERROR(m_bIsOpen != true, "The file stream is already open."); EQFileSystemError eErrorInfo = EQFileSystemError::E_Unknown; const bool FILE_EXISTS = SQFile::Exists(filePath, eErrorInfo); if((FILE_EXISTS && (eOpenMode == EQFileOpenMode::E_Append || eOpenMode == EQFileOpenMode::E_Open)) || (!FILE_EXISTS && eOpenMode == EQFileOpenMode::E_Create) || eOpenMode == EQFileOpenMode::E_CreateOrOverwrite || eOpenMode == EQFileOpenMode::E_OpenOrCreate) { if(FILE_EXISTS) { QFileInfo fileInfo = SQFile::GetFileInfo(filePath, eErrorInfo); m_uFileSize = scast_q(fileInfo.GetSize(), pointer_uint_q); m_bWritingIsAllowed = !fileInfo.IsReadOnly(); QE_ASSERT_ERROR(!(fileInfo.IsReadOnly() && eOpenMode == EQFileOpenMode::E_CreateOrOverwrite), string_q("The current user does not have permissions to overwrite the file \"") + filePath.GetAbsolutePath() + "\"."); } else { m_uFileSize = 0; m_bWritingIsAllowed = true; } m_path = filePath; bool bOperationSuccessful = QFileStream::_OpenPlatformImplementation(m_path, eOpenMode, m_bWritingIsAllowed, m_nativeHandle, eErrorInfo); if(bOperationSuccessful) { m_bIsOpen = true; eErrorInfo = EQFileSystemError::E_Success; if(eOpenMode == EQFileOpenMode::E_Append) this->SetPosition(m_uFileSize); else this->SetPosition(0); QE_ASSERT_WARNING(m_uFileSize <= pointer_uint_q(-1), "The the file is too large, this class cannot access file offsets over 2^32 - 1 when it is compiled for x86 architecture."); if(m_uFileSize > pointer_uint_q(-1)) { eErrorInfo = EQFileSystemError::E_FileIsTooLarge; } } } else if(FILE_EXISTS && eOpenMode == EQFileOpenMode::E_Create) { eErrorInfo = EQFileSystemError::E_AlreadyExists; } else { eErrorInfo = EQFileSystemError::E_DoesNotExist; } return eErrorInfo; }
tuple<double, double, double> pathPlannerFunc(double extend){ const string wcFile = "/home/student/ROVI/Mand2/Kr16WallWorkCell/Scene.wc.xml"; const string deviceName = "KukaKr16"; const string bottle = "Bottle"; Q from(6,-3.142,-0.827,-3.002,-3.143,0.099,-1.573); Q to(6,1.571,0.006,0.030,0.153,0.762,4.490); WorkCell::Ptr wc = WorkCellLoader::Factory::load(wcFile); Device::Ptr device = wc->findDevice(deviceName); rw::kinematics::Frame *deviceB = wc->findFrame(bottle); if (device == NULL) { cerr << "Device: " << deviceName << " not found!" << endl; exit(-1); } if (deviceB == NULL) { cerr << "Device: " << bottle << " not found!" << endl; exit(-1); } rw::kinematics::State state = wc->getDefaultState(); device->setQ(from, state); Kinematics::gripFrame(deviceB,device->getEnd(),state); CollisionDetector detector(wc, ProximityStrategyFactory::makeDefaultCollisionStrategy()); PlannerConstraint constraint = PlannerConstraint::make(&detector,device,state); QSampler::Ptr sampler = QSampler::makeConstrained(QSampler::makeUniform(device),constraint.getQConstraintPtr()); QMetric::Ptr metric = MetricFactory::makeEuclidean<Q>(); QToQPlanner::Ptr planner = RRTPlanner::makeQToQPlanner(constraint, sampler, metric, extend, RRTPlanner::RRTConnect); if (!checkCollisions(device, state, detector, from)) exit(-1); if (!checkCollisions(device, state, detector, to)) exit(-1); PathAnalyzer::CartesianAnalysis distance_traveled; double path_length = 0; PathAnalyzer path_analyse(device,state); QPath path; Timer t; t.resetAndResume(); planner->query(from,to,path,MAXTIME); t.pause(); distance_traveled = path_analyse.analyzeCartesian(path,deviceB); path_length += distance_traveled.length; cout << extend<< "\tPath of length " << path.size() << " found in " << t.getTime() << " seconds." << endl; if (t.getTime() >= MAXTIME) { cout << "Notice: max time of " << MAXTIME << " seconds reached." << endl; } for (QPath::iterator it = path.begin(); it < path.end(); it++) { cout << "set" << *it << endl;//"setQ" << bins.substr(3,bins.length()) << endl; } auto tmp = make_tuple(extend, path_length,t.getTime()); return tmp; }