GraspItServices.h

Go to the documentation of this file.

#ifndef GRASP_PLANNING_GRASPIT_ROS_GRASPITSERVICES_H
#define GRASP_PLANNING_GRASPIT_ROS_GRASPITSERVICES_H

#include <ros/ros.h>
#include <string>
#include <vector>

#include <grasp_planning_graspit/SharedPtr.h>

#include <grasp_planning_graspit_msgs/LoadDatabaseModel.h>
#include <grasp_planning_graspit_msgs/AddToDatabase.h>
#include <grasp_planning_graspit_msgs/SaveWorld.h>

#include <moveit_msgs/GraspPlanning.h>

namespace GraspIt
{

class GraspItSceneManager;
class GraspItSimpleDBManager;
class EigenGraspPlanner;
class EigenGraspResult;

class GraspItServices
{
public:
    GraspItServices();
    ~GraspItServices();

    void start();

private:
    GraspItServices(const GraspItServices& o) {}

    void init();

    // reads parameters from the parameter service
    void readParams();

    bool acceptAddToDB(grasp_planning_graspit_msgs::AddToDatabase::Request &req,
                       grasp_planning_graspit_msgs::AddToDatabase::Response &res);

    bool acceptLoadModel(grasp_planning_graspit_msgs::LoadDatabaseModel::Request &req,
                         grasp_planning_graspit_msgs::LoadDatabaseModel::Response &res);

    bool acceptSaveWorld(grasp_planning_graspit_msgs::SaveWorld::Request &req,
                         grasp_planning_graspit_msgs::SaveWorld::Response &res);

    bool acceptEGPlanning(moveit_msgs::GraspPlanning::Request &req,
                          moveit_msgs::GraspPlanning::Response &res);

    moveit_msgs::Grasp getGraspMsg(const EigenGraspResult& r, const std::string& id,
                                         const std::vector<std::string>& robotJointNames,
                                         const std::string& objectFrame) const;


    SHARED_PTR<GraspItSceneManager> graspitMgr;
    SHARED_PTR<GraspItSimpleDBManager> mgr;
    SHARED_PTR<GraspIt::EigenGraspPlanner> egPlanner;

    // ---- service servers ---

    ros::ServiceServer addGraspitFileSrv;
    ros::ServiceServer loadGraspitModelSrv;
    ros::ServiceServer saveWorldSrv;
    ros::ServiceServer egPlanningSrv;

    // --- variables read from ROS parameter server (commented in class header)

    std::string dbName;
    std::string egPlannerName;
    std::string addToDBTopic;
    std::string loadModelTopic;
    std::string saveWorldTopic;
    std::string egPlanningTopic;
    int defaultMaxPlanningSteps;
    int defaultNumRepeatPlanning;
    int defaultNumKeepResults;
    bool defaultFinishWithAutograsp;
    float graspMsgPositionFactor;
    std::string resultsOutputDirectory;
    bool saveResultFilesInventor;
    bool saveResultFilesGraspit;


    // if true, the joint DOF values will be generated. This may be necessary
    // because graspit generates results with opposite values
    bool negateJointDOFs;


    ros::NodeHandle priv;
    ros::NodeHandle pub;
};

}  //  namespace GraspIt

#endif  // GRASP_PLANNING_GRASPIT_ROS_GRASPITSERVICES_H