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