grasp_planning_node.cpp

Go to the documentation of this file.

#ifdef DOXYGEN_SHOULD_SKIP_THIS

#endif  // DOXYGEN_SHOULD_SKIP_THIS

#include <string>
#include <set>
#include <vector>

#include <stdio.h>
#include <execinfo.h>
#include <signal.h>
#include <stdlib.h>
#include <unistd.h>

#include <ros/ros.h>
#include <grasp_planning_graspit/GraspItSceneManagerHeadless.h>
#include <grasp_planning_graspit/LogBinding.h>
#include <grasp_planning_graspit/EigenGraspPlanner.h>
#include <grasp_planning_graspit/EigenGraspResult.h>
#include <grasp_planning_graspit_ros/LogBindingROS.h>


void print_trace(void)
{
    void *array[10];
    size_t size;
    char **strings;
    size_t i;

    size = backtrace(array, 10);
    strings = backtrace_symbols(array, size);

    printf("Obtained %zd stack frames.\n", size);

    for (i = 0; i < size; i++)
        printf("%s\n", strings[i]);

    free(strings);
}


void handler(int sig)
{
    print_trace();
    ros::shutdown();
    exit(1);
}

int main(int argc, char **argv)
{
    signal(SIGSEGV, handler);
    signal(SIGABRT, handler);

    ros::init(argc, argv, "grasp_planning_graspit", ros::init_options::AnonymousName);
    ros::NodeHandle priv("~");
    ros::NodeHandle pub("");


    bool useRosLogging = true;
    if (useRosLogging)
    {
        PRINT_INIT_ROS();
    }
    else
    {
        PRINT_INIT_STD();
    }


    ROS_INFO_STREAM("Reading parameters from namespace " << priv.getNamespace());

    std::string outputDirectory;
    if (!priv.hasParam("results_output_directory"))
    {
        PRINTERROR("Could not read parameter results_output_directory");
        return 1;
    }
    priv.param<std::string>("results_output_directory", outputDirectory, outputDirectory);
    PRINTMSG("Results output directory: " << outputDirectory);

    bool useWorld = false;
    if (!priv.hasParam("use_world_file"))
    {
        PRINTERROR("Could not read parameter use_world_file");
        return 1;
    }
    priv.param<bool>("use_world_file", useWorld, useWorld);

    std::string worldFilename;
    std::string robotFilename;
    std::string objectFilename;

    if (useWorld)
    {
        PRINTMSG("Using world file");
        if (!priv.hasParam("graspit_world_file"))
        {
            PRINTERROR("Could not read parameter graspit_world_file");
            return 1;
        }
        priv.param<std::string>("graspit_world_file", worldFilename, worldFilename);
        PRINTMSG("Using world file " << worldFilename);
    }
    else
    {
        PRINTMSG("Using robot and object file");
        if (!priv.hasParam("graspit_robot_file"))
        {
            PRINTERROR("Could not read parameter graspit_robot_file");
            return 1;
        }
        priv.param<std::string>("graspit_robot_file", robotFilename, robotFilename);
        PRINTMSG("Using robot file " << robotFilename);

        if (!priv.hasParam("graspit_object_file"))
        {
            PRINTERROR("Could not read parameter graspit_object_file");
            return 1;
        }
        priv.param<std::string>("graspit_object_file", objectFilename, objectFilename);
    }

    int maxPlanningSteps;
    priv.param<int>("max_planning_steps", maxPlanningSteps, 50000);
    PRINTMSG("Using max number of planning steps: " << maxPlanningSteps);

    int repeatPlanning;
    priv.param<int>("num_repeat_planning", repeatPlanning, 1);
    PRINTMSG("Repeating planning: " << repeatPlanning);


    int numKeepResults;
    priv.param<int>("default_num_keep_results", numKeepResults, 5);
    PRINTMSG("Using default number of results kept: " << numKeepResults);
    
    bool finishWithAutograsp;
    priv.param<bool>("default_finish_with_autograsp", finishWithAutograsp, false);
    PRINTMSG("Finish with auto-grasp by default: " << finishWithAutograsp);

    bool saveResultFilesInventor;
    priv.param<bool>("save_result_files_inventor", saveResultFilesInventor, true);
    PRINTMSG("Save result files inventor: " << saveResultFilesInventor);

    bool saveResultFilesGraspit;
    priv.param<bool>("save_result_files_graspit", saveResultFilesGraspit, true);
    PRINTMSG("Save result files graspit: " << saveResultFilesGraspit);

    PRINTMSG("Creating planner");
    std::string name = "EigenGraspPlanner1";  // TODO make parameter
    SHARED_PTR<GraspIt::GraspItSceneManager> graspitMgr(new GraspIt::GraspItSceneManagerHeadless());

    SHARED_PTR<GraspIt::EigenGraspPlanner> p(new GraspIt::EigenGraspPlanner(name, graspitMgr));

    if (!worldFilename.empty())
    {
        PRINTMSG("Loading world");
        graspitMgr->loadWorld(worldFilename);
    }
    else
    {
        // TODO add an option to specify a transform for the object.
        // For now, they're put in the origin. For the planning, this should not really matter...
        GraspIt::EigenTransform robotTransform;
        GraspIt::EigenTransform objectTransform;
        robotTransform.setIdentity();
        objectTransform.setIdentity();
        // objectTransform.translate(Eigen::Vector3d(100,0,0));
        std::string robotName("Robot1");  // TODO parameterize
        std::string objectName("Object1");  // TODO parameterize
        if ((graspitMgr->loadRobot(robotFilename, robotName, robotTransform) != 0) ||
                (graspitMgr->loadObject(objectFilename, objectName, true, objectTransform) != 0))
        {
            PRINTERROR("Could not load robot or object");
            return 0;
        }

        // in case one wants to view the initial world before planning, save it:
        graspitMgr->saveGraspItWorld(outputDirectory + "/worlds/startWorld.xml", true);
        graspitMgr->saveInventorWorld(outputDirectory + "/worlds/startWorld.iv", true);
    }

    PRINTMSG("Now planning...");
    if (!p->plan(maxPlanningSteps, repeatPlanning, numKeepResults, finishWithAutograsp))
    {
        PRINTERROR("Could not plan.");
        return 0;
    }

    PRINTMSG("Saving results as world files");

    std::string resultsWorldDirectory = outputDirectory + "/worlds";
    std::string filenamePrefix = "world";
    p->saveResultsAsWorldFiles(resultsWorldDirectory, filenamePrefix, true, true);

    std::vector<GraspIt::EigenGraspResult> allGrasps;
    p->getResults(allGrasps);

    PRINTMSG("Grasp results:");
    std::vector<GraspIt::EigenGraspResult>::iterator it;
    for (it = allGrasps.begin(); it != allGrasps.end(); ++it)
    {
        PRINTMSG(*it);
    }


    PRINTMSG("Quitting program.");
    return 0;
}