BOOL LLVFile::rename(const LLUUID &new_id, const LLAssetType::EType new_type)
{
if (! (mMode & WRITE))
{
llwarns << "Attempt to rename file " << mFileID << " opened with mode " << std::hex << mMode << std::dec << llendl;
return FALSE;
}
if (mHandle != LLVFSThread::nullHandle())
{
llwarns << "Renaming file with pending async read" << llendl;
}
waitForLock(VFSLOCK_READ);
waitForLock(VFSLOCK_APPEND);
// we need to release / replace our own lock
// since the renamed file will inherit locks from the new name
mVFS->decLock(mFileID, mFileType, VFSLOCK_OPEN);
mVFS->renameFile(mFileID, mFileType, new_id, new_type);
mVFS->incLock(new_id, new_type, VFSLOCK_OPEN);
mFileID = new_id;
mFileType = new_type;
return TRUE;
}
BOOL LLVFile::remove()
{
// llinfos << "Removing file " << mFileID << llendl;
if (! (mMode & WRITE))
{
// Leaving paranoia warning just because this should be a very infrequent
// operation.
llwarns << "Remove file " << mFileID << " opened with mode " << std::hex << mMode << std::dec << llendl;
}
if (mHandle != LLVFSThread::nullHandle())
{
llwarns << "Removing file with pending async read" << llendl;
}
// why not seek back to the beginning of the file too?
mPosition = 0;
waitForLock(VFSLOCK_READ);
waitForLock(VFSLOCK_APPEND);
mVFS->removeFile(mFileID, mFileType);
return TRUE;
}
S32 LLVFile::getSize()
{
waitForLock(VFSLOCK_APPEND);
S32 size = mVFS->getSize(mFileID, mFileType);
return size;
}
void Controller::returnToOriginalPosition()
{
moveHead(0.0, 0.0);
mLock = LOCK_PATH;
moveBase(mOriginalPosition.pose);
waitForLock();
}
BOOL LLVFile::write(const U8 *buffer, S32 bytes)
{
if (! (mMode & WRITE))
{
llwarns << "Attempt to write to file " << mFileID << " opened with mode " << std::hex << mMode << std::dec << llendl;
}
if (mHandle != LLVFSThread::nullHandle())
{
llerrs << "Attempt to write to vfile object " << mFileID << " with pending async operation" << llendl;
return FALSE;
}
BOOL success = TRUE;
// *FIX: allow async writes? potential problem wit mPosition...
if (mMode == APPEND) // all appends are async (but WRITEs are not)
{
U8* writebuf = new U8[bytes];
memcpy(writebuf, buffer, bytes);
S32 offset = -1;
mHandle = sVFSThread->write(mVFS, mFileID, mFileType,
writebuf, offset, bytes,
LLVFSThread::FLAG_AUTO_COMPLETE | LLVFSThread::FLAG_AUTO_DELETE);
mHandle = LLVFSThread::nullHandle(); // FLAG_AUTO_COMPLETE means we don't track this
}
else
{
// We can't do a write while there are pending reads or writes on this file
waitForLock(VFSLOCK_READ);
waitForLock(VFSLOCK_APPEND);
S32 pos = (mMode & APPEND) == APPEND ? -1 : mPosition;
S32 wrote = sVFSThread->writeImmediate(mVFS, mFileID, mFileType, (U8*)buffer, pos, bytes);
mPosition += wrote;
if (wrote < bytes)
{
llwarns << "Tried to write " << bytes << " bytes, actually wrote " << wrote << llendl;
success = FALSE;
}
}
return success;
}
BOOL LLVFile::read(U8 *buffer, S32 bytes, BOOL async, F32 priority)
{
if (! (mMode & READ))
{
llwarns << "Attempt to read from file " << mFileID << " opened with mode " << std::hex << mMode << std::dec << llendl;
return FALSE;
}
if (mHandle != LLVFSThread::nullHandle())
{
llwarns << "Attempt to read from vfile object " << mFileID << " with pending async operation" << llendl;
return FALSE;
}
mPriority = priority;
BOOL success = TRUE;
// We can't do a read while there are pending async writes
waitForLock(VFSLOCK_APPEND);
// *FIX: (???)
if (async)
{
mHandle = sVFSThread->read(mVFS, mFileID, mFileType, buffer, mPosition, bytes, threadPri());
}
else
{
// We can't do a read while there are pending async writes on this file
mBytesRead = sVFSThread->readImmediate(mVFS, mFileID, mFileType, buffer, mPosition, bytes);
mPosition += mBytesRead;
if (! mBytesRead)
{
success = FALSE;
}
}
return success;
}
Lock::GlobalLock::GlobalLock(Locker* locker, LockMode lockMode, unsigned timeoutMs)
: GlobalLock(locker, lockMode, EnqueueOnly()) {
waitForLock(timeoutMs);
}
uint8_t Controller::get(int object)
{
mBusy = true;
float min_y = 0.f;
mLock = LOCK_ARM;
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
waitForLock();
if (mPathingEnabled)
{
//Go to the table
updateRobotPosition();
moveHead(0.0, 0.0);
mLock = LOCK_PATH;
moveBase(mGoal);
waitForLock();
ROS_INFO("Reached Goal");
}
//Aim Kinect to the table
setFocusFace(false);
mLock = LOCK_HEAD;
moveHead(VIEW_OBJECTS_ANGLE, 0.0);
waitForLock();
//Start object recognition process
geometry_msgs::PoseStamped objectPose;
double drive_time = 0.0;
uint8_t attempts = 0;
for (attempts = 0; attempts < MAX_GRAB_ATTEMPTS; attempts++)
{
// start depth for object grabbing (takes a second to start up, so we put it here)
setDepthForced(true);
uint8_t i = 0;
for (i = 0; i < 3; i++)
{
mLock = LOCK_HEAD;
switch (i)
{
case 0: moveHead(VIEW_OBJECTS_ANGLE, 0.0); break;
case 1: moveHead(VIEW_OBJECTS_ANGLE, MIN_VIEW_ANGLE); break;
case 2: moveHead(VIEW_OBJECTS_ANGLE, MAX_VIEW_ANGLE); break;
}
waitForLock();
if (findObject(object, objectPose, min_y) && objectPose.pose.position.y != 0.0)
{
// object found with turned head? rotate base
if (i != 0)
{
mLock = LOCK_BASE;
moveHead(VIEW_OBJECTS_ANGLE, 0.0);
switch (i)
{
case 1: rotateBase(MIN_VIEW_ANGLE); break;
case 2: rotateBase(MAX_VIEW_ANGLE); break;
}
waitForLock();
if (findObject(object, objectPose, min_y) == false || objectPose.pose.position.y == 0.0)
{
ROS_ERROR("Failed to find object, quitting script.");
if (mPathingEnabled)
returnToOriginalPosition();
setDepthForced(false);
return nero_msgs::Emotion::SAD;
}
}
break;
}
}
// found an object, now rotate base to face the object directly and
// create enough space between the robot and the table for the arm to move up
double yaw = -atan(objectPose.pose.position.x / objectPose.pose.position.y);
while (fabs(yaw) > TARGET_YAW_THRESHOLD || fabs(TABLE_DISTANCE - min_y) > TABLE_DISTANCE_THRESHOLD)
{
if (fabs(TABLE_DISTANCE - min_y) > TABLE_DISTANCE_THRESHOLD)
{
ROS_INFO("Moving by %lf. Distance: %lf", min_y - TABLE_DISTANCE, min_y);
mLock = LOCK_BASE;
positionBase(min_y - TABLE_DISTANCE);
waitForLock();
}
else
{
ROS_INFO("Rotating by %lf.", yaw);
mLock = LOCK_BASE;
rotateBase(yaw);
waitForLock();
}
if (findObject(object, objectPose, min_y) == false || objectPose.pose.position.y == 0.0)
{
ROS_ERROR("Failed to find object, quitting script.");
if (mPathingEnabled)
returnToOriginalPosition();
return nero_msgs::Emotion::SAD;
}
yaw = -atan(objectPose.pose.position.x / objectPose.pose.position.y);
}
setDepthForced(false); // we are done with object finding
ROS_INFO("yaw: %lf", yaw);
ROS_INFO("distance: %lf", fabs(TABLE_DISTANCE - min_y));
//Move arm to object and grab it
objectPose.pose.position.x = 0.0; // we are straight ahead of the object, so this should be 0.0 (it is most likely already close to 0.0)
mLock = LOCK_ARM;
moveArm(objectPose);
waitForLock();
//Be ready to grab object
setGripper(true);
usleep(1000000);
setGripper(false);
//Move forward to grab the object
double expected_drive_time = (objectPose.pose.position.y - ARM_LENGTH) / GRAB_TARGET_SPEED;
drive_time = positionBaseSpeed(expected_drive_time + EXTRA_GRAB_TIME, GRAB_TARGET_SPEED, true);
if (drive_time >= expected_drive_time + EXTRA_GRAB_TIME)
{
ROS_ERROR("Been driving forward too long!");
// open gripper again
setGripper(true);
expressEmotion(nero_msgs::Emotion::SAD);
positionBaseSpeed(drive_time, -GRAB_TARGET_SPEED);
//Move object to body
mLock = LOCK_ARM;
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
waitForLock();
expressEmotion(nero_msgs::Emotion::NEUTRAL);
}
else
break;
}
// no more attempts left, we are sad
if (attempts >= MAX_GRAB_ATTEMPTS)
{
ROS_ERROR("No more grab attempts left.");
if (mPathingEnabled)
returnToOriginalPosition();
return nero_msgs::Emotion::SAD;
}
// little hack to allow the gripper to close
usleep(500000);
//Lift object
mLock = LOCK_ARM;
moveArm(objectPose.pose.position.x, objectPose.pose.position.z + LIFT_OBJECT_DISTANCE);
waitForLock();
//Move away from table
positionBaseSpeed(drive_time, -GRAB_TARGET_SPEED);
//Move object to body
mLock = LOCK_ARM;
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
waitForLock();
if (mPathingEnabled)
returnToOriginalPosition();
moveHead(FOCUS_FACE_ANGLE, 0.0);
//Deliver the juice
//mLock = LOCK_ARM;
//moveArm(DELIVER_ARM_X_VALUE, DELIVER_ARM_Z_VALUE);
//waitForLock();
mBusy = false;
return nero_msgs::Emotion::HAPPY;
}
