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;
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 9
0
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;
}