/**
* Delete an object from the file system (SD card).
* @param[in] obj The object handle.
* @param[in] instId The object instance
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
PIOS_FLASHFS_ObjDelete(obj, instId);
#endif
#if defined(PIOS_INCLUDE_SDCARD)
ObjectList *objEntry;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(objEntry, filename);
// Delete file
PIOS_FUNLINK(filename);
// Done
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
}
/**
* Save the data of the specified object to the file system (SD card).
* If the object contains multiple instances, all of them will be saved.
* A new file with the name of the object will be created.
* The object data can be restored using the UAVObjLoad function.
* @param[in] obj The object handle.
* @param[in] instId The instance ID
* @param[in] file File to append to
* @return 0 if success or -1 if failure
*/
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
ObjectList *objEntry = (ObjectList *) obj;
if (objEntry == NULL)
return -1;
ObjectInstList *instEntry = getInstance(objEntry, instId);
if (instEntry == NULL)
return -1;
if (instEntry->data == NULL)
return -1;
if (PIOS_FLASHFS_ObjSave(obj, instId, instEntry->data) != 0)
return -1;
#endif
#if defined(PIOS_INCLUDE_SDCARD)
FILEINFO file;
ObjectList *objEntry;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(objEntry, filename);
// Open file
if (PIOS_FOPEN_WRITE(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Append object
if (UAVObjSaveToFile(obj, instId, &file) == -1) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
}
int run(int argc, char **argv)
{
if (argc < 4)
{
PRINTMSG("Usage: " << argv[0] << " <outputDir> <robotFile> <objectFile>");
return 0;
}
std::string outputDirectory(argv[1]);
std::string robotFilename(argv[2]);
std::string objectFilename(argv[3]);
PRINTMSG("Creating database");
std::string name = "Database1";
SHARED_PTR<GraspIt::GraspItSceneManager> graspitMgr(new GraspIt::GraspItSceneManagerHeadless());
SHARED_PTR<GraspIt::GraspItSimpleDBManager> mgr(new GraspIt::GraspItSimpleDBManager(name, graspitMgr));
std::string robotName("Robot1");
std::string objectName("Object1");
PRINTMSG("Now loading robot");
int robotID = -1;
int objectID = -1;
// here we'd have to put the robot joint names, we'll leave this empty for this test fle
std::vector<std::string> jointNames;
if ((robotID = mgr->loadRobotToDatabase(robotFilename, robotName, jointNames)) < 0)
{
PRINTERROR("Could not load robot");
return 0;
}
PRINTMSG("Now loading object");
if ((objectID = mgr->loadObjectToDatabase(objectFilename, objectName, true)) < 0)
{
PRINTERROR("Could not load object");
return 0;
}
// Now objects should be in the database only, but not in the world.
// Re-add them to test if the world still workds afterwards:
PRINTMSG("Now loading robot to world");
if (mgr->loadToWorld(robotID, GraspIt::EigenTransform::Identity()) != 0)
{
PRINTERROR("Could not add the robot to the graspit world");
return 0;
}
PRINTMSG("Now loading object to world");
if (mgr->loadToWorld(objectID, GraspIt::EigenTransform::Identity()) != 0)
{
PRINTERROR("Could not add the object to the graspit world");
return 0;
}
PRINTMSG("Saving world files");
// test to see if it worked: save as world
bool createDir = true; // if true, the directory will be created, if it doesn't exist.
graspitMgr->saveGraspItWorld(outputDirectory + "/dbtest/world.xml", createDir);
graspitMgr->saveInventorWorld(outputDirectory + "/dbtest/world.iv", createDir);
PRINTMSG("Quitting program.");
return 0;
}
/**
* Load an object from the file system (SD card).
* A file with the name of the object will be opened.
* The object data can be saved using the UAVObjSave function.
* @param[in] obj The object handle.
* @param[in] instId The object instance
* @return 0 if success or -1 if failure
*/
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
{
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
ObjectList *objEntry = (ObjectList *) obj;
if (objEntry == NULL)
return -1;
ObjectInstList *instEntry = getInstance(objEntry, instId);
if (instEntry == NULL)
return -1;
if (instEntry->data == NULL)
return -1;
// Fire event on success
if (PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data) == 0)
sendEvent(objEntry, instId, EV_UNPACKED);
else
return -1;
#endif
#if defined(PIOS_INCLUDE_SDCARD)
FILEINFO file;
ObjectList *objEntry;
UAVObjHandle loadedObj;
ObjectList *loadedObjEntry;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(objEntry, filename);
// Open file
if (PIOS_FOPEN_READ(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Load object
loadedObj = UAVObjLoadFromFile(&file);
if (loadedObj == 0) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Check that the IDs match
loadedObjEntry = (ObjectList *) loadedObj;
if (loadedObjEntry->id != objEntry->id) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
}
