bool CFavourites::AddOrRemove(CFileItem *item, int contextWindow)
{
if (!item) return false;
// load our list
CFileItemList items;
Load(items);
CStdString executePath(GetExecutePath(item, contextWindow));
CFileItemPtr match = items.Get(executePath);
if (match)
{ // remove the item
items.Remove(match.get());
}
else
{ // create our new favourite item
CFileItemPtr favourite(new CFileItem(item->GetLabel()));
if (item->GetLabel().IsEmpty())
favourite->SetLabel(CUtil::GetTitleFromPath(item->m_strPath, item->m_bIsFolder));
favourite->SetThumbnailImage(item->GetThumbnailImage());
favourite->m_strPath = executePath;
items.Add(favourite);
}
// and save our list again
return Save(items);
}
bool CFavouritesDirectory::AddOrRemove(CFileItem *item, int contextWindow)
{
if (!item) return false;
// load our list
CFileItemList items;
Load(items);
std::string executePath(GetExecutePath(*item, contextWindow));
CFileItemPtr match = items.Get(executePath);
if (match)
{ // remove the item
items.Remove(match.get());
}
else
{ // create our new favourite item
CFileItemPtr favourite(new CFileItem(item->GetLabel()));
if (item->GetLabel().empty())
favourite->SetLabel(CUtil::GetTitleFromPath(item->GetPath(), item->m_bIsFolder));
favourite->SetArt("thumb", item->GetArt("thumb"));
favourite->SetPath(executePath);
items.Add(favourite);
}
// and save our list again
return Save(items);
}
void FabAtHomePrinter::executePausePath(const Point& start)
{
vector<Point> pausePathPoints(2);
//Move the platform down.
pausePathPoints.clear();
pausePathPoints.push_back(start);
pausePathPoints.push_back(Point(start.x, start.y, start.z + PLATFORM_DELTA));
executePath(Path(NULL,pausePathPoints),NULL,false);
paused = true;
Util::messageBox("Execution has been paused. Press OK to resume execution.");
paused = false;
//Move the platform up.
pausePathPoints.clear();
pausePathPoints.push_back(Point(start.x, start.y, start.z + PLATFORM_DELTA));
pausePathPoints.push_back(start);
executePath(Path(NULL,pausePathPoints),NULL,false);
}
void FabAtHomePrinter::executeSetupPath(const Point& start, const Point& end, const double& clearance)
{
vector<Point> setupPathPoints(4);
setupPathPoints.clear();
setupPathPoints.push_back(start);
setupPathPoints.push_back(Point(start.x, start.y, start.z - clearance));
setupPathPoints.push_back(Point(end.x, end.y, start.z - clearance));
setupPathPoints.push_back(end);
executePath(Path(NULL,setupPathPoints),NULL,false);
}
bool MoveitPlanningInterface::moveArmsBack(void) {
gripper.gripper_open();
left_arm.setPoseTarget(left_arm_back_pose);
left_arm.move();
if(planPath(arm_back_joints)){
if(!executePath()) {
return false;
}
} else {
return false;
}
return true;
}
void cServant::Update(double dt)
{
if (route.empty())
{
patrolPath.location = 0;
route = pathFind(currTile.x, currTile.y,
patrolPath.WayPointTileList[patrolPath.location].x,
patrolPath.WayPointTileList[patrolPath.location].y);
}
if ((gotoNavi || gotoRoam) && routeCounter == 0 && routeCounter2 == 0 && routeCounter3 == 0)
{
if (gotoNavi)
{
route3 = "";
routeCounter3 = 0;
hasSetDest2 = false;
}
else if (gotoRoam)
{
route = "";
routeCounter = 0;
patrolPath.location = 0;
AI_STATE = AS_ROAM;
}
}
switch (AI_STATE)
{
case cServant::AS_ROAM:
gotoRoam = false;
if (currTile.x == patrolPath.WayPointTileList[patrolPath.location].x &&
currTile.y == patrolPath.WayPointTileList[patrolPath.location].y &&
routeCounter == 0 && rotating == false)
{
patrolPath.location++;
if (patrolPath.location >= patrolPath.WayPointTileList.size())
patrolPath.location = 0;
route = pathFind(currTile.x, currTile.y,
patrolPath.WayPointTileList[patrolPath.location].x,
patrolPath.WayPointTileList[patrolPath.location].y);
}
executePath(dt, route, routeCounter);
if (gotoServe && routeCounter == 0)
{
route2 = "";
routeCounter2 = 0;
hasSetDest = false;
}
break;
default:
break;
}
}
void FabAtHomePrinter::print(System::ComponentModel::BackgroundWorker* fabricationThread, string& displayText)
{
printing = true;
pausePrintFlag = false;
cancelPrintFlag = false;
redoPathFlag = false;
initializePathMode();
unsigned int i = 0;
if(fabricationThread != NULL)
fabricationThread->ReportProgress(i);
//Iterate through the loaded paths and execute them.
while(i < model.paths.size())
{
Path path = model.paths[i]; //Copy construct the path to be executed.
//Find a bay with the required material calibration.
Bay* bay = findBay(path.materialCalibration);
while(bay == NULL)
{
//Prompt user to load required material calibration into a bay.
Util::messageBox("No bay has the required material calibration. Press OK to pause execution.");
Point start(axes["X"].motor->getPosition(), axes["Y"].motor->getPosition(), axes["Z"].motor->getPosition()); //The current position.
executePausePath(start);
bay = findBay(path.materialCalibration);
}
//Apply bay offset to path.
for(vector<Point>::iterator j = path.points.begin(); j != path.points.end(); ++j)
{
j->x -= bay->location.x;
j->y -= bay->location.y;
j->z -= bay->location.z;
}
//Update the display text.
if(fabricationThread != NULL)
{
displayText = "Executing setup path.";
fabricationThread->ReportProgress(i);
}
//Move from the current position to the beginning of the current path.
Point start(axes["X"].motor->getPosition(), axes["Y"].motor->getPosition(), axes["Z"].motor->getPosition()); //The current position.
Point end = path.points[0]; //The first point of the current path.
executeSetupPath(start, end, path.materialCalibration->CLEARANCE);
//Update the display text.
if(fabricationThread != NULL)
{
displayText = "Executing path "+Util::toString<int>(i+1)+" of "+Util::toString<int>(model.paths.size())+".";
fabricationThread->ReportProgress(i);
}
//Execute the current path.
executePath(path,bay,true);
++(path.materialCalibration->pathsExecuted); //Increment the number of paths executed by the material calibration.
if((path.materialCalibration->pathsExecuted % (int)(path.materialCalibration->PAUSE_PATHS)) == 0)
{
//An automatic pause has been trigger.
pausePrintFlag = true;
}
if(pausePrintFlag)
{
//Process a manual or automatic pause.
executePausePath(path.points[path.points.size()-1]);
pausePrintFlag = false;
}
if(cancelPrintFlag)
{
//Cancel the print.
break;
}
if(redoPathFlag)
{
//Redo the current path.
redoPathFlag = false;
}
else
{
//Move on to the next path.
++i;
if(fabricationThread != NULL)
fabricationThread->ReportProgress(i);
}
}
//Update the display text.
if (cancelPrintFlag)
{
cancelPrintFlag = false;
if(fabricationThread != NULL)
displayText = "Fabrication cancelled.";
}
else
{
if(fabricationThread != NULL)
displayText = "Fabrication successfully completed.";
}
if(fabricationThread != NULL)
fabricationThread->ReportProgress(i);
//Execution has stopped. Move the platform down.
Point start(axes["X"].motor->getPosition(), axes["Y"].motor->getPosition(), axes["Z"].motor->getPosition()); //The current position.
Point end(start.x, start.y, start.z - PLATFORM_DELTA);
executeSetupPath(start,end,0);
printing = false;
}
// ######################################################################
int submain(const int argc, const char** argv)
{
// catch signals and redirect them for a clean exit (in particular, this gives us a chance to do useful things like
// flush and close output files that would otherwise be left in a bogus state, like mpeg output files):
catchsignals(&signum);
LINFO("#############################################STARTING##############################################");
ModelManager mgr("App Scorbot MultiGrab");
scorbot.reset(new ScorbotSimple(mgr));
mgr.addSubComponent(scorbot);
setupGrabbers(mgr);
ofs.reset(new OutputFrameSeries(mgr));
mgr.addSubComponent(ofs);
if(mgr.parseCommandLine(argc, argv, "FilePrefix SceneID", 2, 2) == false) return -1;
mgr.start();
if (grabbers.size() < NUMCAMS) LFATAL("Only found %" ZU " cameras instead of %d. Reboot your machine and try again.", grabbers.size(), NUMCAMS);
// get our grabbers to start grabbing:
for (size_t cameraID = 0; cameraID < grabbers.size(); ++cameraID)
grabberThreadServer.enqueueJob(rutz::make_shared(new GrabJob(cameraID)));
sceneDir = mgr.getExtraArg(0);
sceneID = boost::lexical_cast<int>(mgr.getExtraArg(1));
pthread_t displayThread;
pthread_create(&displayThread, NULL, &displayThreadMethod, NULL);
// Create the interactive window
userInteractiveWindow = new XWinManaged(Dims(640,480), -1, -1, "User Interactive");
userInteractiveWindow->setVisible(false);
userInteractiveWindow->setPosition(0, 0);
// Main loop:
int runnumber = 0;
while(true) {
if(signum != 0) break;
// home the robot once in a while:
if ((runnumber % 5) == 0) {
int gogo = 0; getInt("Perform robot homing sequence and press ENTER", gogo);
}
// select the scene:
getInt("Enter scene ID (-1 to exit):", sceneID);
if (sceneID == -1) break; // abort on scene -1
// STEP 1. Load the scene file
SceneSetup setup = loadSceneSetup(sceneID);
// STEP 2. Show the interactive window:
userInteractiveWindow->setVisible(true);
// STEP 3. Display background image and ask the user to place it on the scene
Image< PixRGB<byte> > backgroundImage = Raster::ReadRGB(setup.setupPath + "/" + setup.backgroundFileName);
backgroundImage = rescale(backgroundImage, Dims(640, 480));
writeText(backgroundImage, Point2D<int>(0, 0), "Please place this background on the scene and press ENTER.");
userInteractiveWindow->drawImage(backgroundImage, 0, 0, true);
LINFO("Place background map on scene and add houses, trees, and other background objects. See User Interactive window for instructions...");
// eat all previous mouse clicks and key presses, just in case:
while (userInteractiveWindow->getLastMouseClick() != Point2D<int>(-1, -1)) { }
while (userInteractiveWindow->getLastKeyPress() != -1) { }
// wait for ENTER:
while(userInteractiveWindow->getLastKeyPress() != 36) usleep(100000);
LINFO("Background map done. Make sure you have built a nice scene.");
// STEP 4. Display each object and ask user to put on the scene and specify its bounding box
for (size_t i = 0; i < setup.objects.size(); ++i)
setup.objects[i].outline = promptPlaceObjectOnScene(setup, i);
// STEP 5. Hide the interactive window
userInteractiveWindow->setVisible(false);
// STEP 6. Write out outlines to a file
{
std::string objectFileName = sformat("%s/RobotScene-s%04d-polygons.txt", dataDir, sceneID);
std::ofstream objectFile(objectFileName.c_str());
LINFO("Saving the object bounding boxes into: %s", objectFileName.c_str());
for(size_t i=0; i<setup.objects.size(); ++i)
{
for(size_t j=0; j<setup.objects[i].outline.size(); ++j)
{
Point2D<int> &pnt = setup.objects[i].outline[j];
if(j != 0) objectFile << ',';
objectFile << pnt.i << ',' << pnt.j;
}
objectFile << std::endl;
}
}
// STEP 7. Execute the path and record the videos
for (pathID = 0; pathID < int(setup.pathIndex.size()); ++pathID) {
if(signum != 0) break;
// create a directory for this scene / light:
const std::string dir = sformat("%s/RobotScene-s%04d-p%02d", dataDir, sceneID, pathID);
const std::string cmd = sformat("/bin/mkdir -p %s", dir.c_str());
if (system(cmd.c_str()) == -1) PLFATAL("Could not create directory %s", dir.c_str());
int gogo = pathID; getInt("Set light and press ENTER to start video recording", gogo);
// make sure we don't have too many pending disk writes:
while(writer.size() > 1000) {
LINFO("Waiting for image writer thread, queue size = %" ZU "...", writer.size());
usleep(1000000);
}
LINFO("Running Scene %04d Path %02d ...", sceneID, setup.pathIndex[pathID]);
executePath(setup.pathIndex[pathID]);
}
if(signum != 0) break;
// STEP 8. Instruct users to place the objects back into the bins
userInteractiveWindow->setVisible(true);
userInteractiveWindow->setPosition(0, 0);
for(size_t i=0; i<setup.objects.size(); ++i) promptReturnObjectToTray(setup, i);
userInteractiveWindow->setVisible(false);
// STEP 9. Ready for next scene
++sceneID;
++runnumber;
}
// stop grabbing:
keepgoing = false;
// wait for all pics to be written (note: this just waits until the queue of pending jobs is empty, the writer's
// destructor will wait until all jobs are complete):
while(writer.size()) {
LINFO("Waiting for image writer thread, queue size = %" ZU "...", writer.size());
usleep(500000);
}
writer.flushQueue(250000, true);
// make sure all is done
LINFO("Cleaning up... Stand by...");
usleep(2000000);
// stop all our ModelComponents
mgr.stop();
LINFO("Finished.");
return 0;
}
