ros_graspit_interface.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#ifndef _ROS_GRASPIT_INTERFACE_H_
#define _ROS_GRASPIT_INTERFACE_H_

#include <map>

//GraspIt! includes
#include <include/plugin.h>
namespace db_planner
{
class DatabaseManager;
}
class GraspitDBModel;
class Pr2Gripper2010;
class Body;
class transf;
class GraspableBody;

#include <ros/ros.h>

#include <manipulation_msgs/Grasp.h>
#include <manipulation_msgs/GraspPlanning.h>

#include "graspit_ros_planning_msgs/LoadDatabaseModel.h"
#include "graspit_ros_planning_msgs/LoadObstacle.h"
#include "graspit_ros_planning_msgs/ClearBodies.h"
#include "graspit_ros_planning_msgs/SimulateScan.h"
#include "graspit_ros_planning_msgs/TestGrasp.h"
#include "graspit_ros_planning_msgs/GenerateGrasp.h"
#include "graspit_ros_planning_msgs/VerifyGrasp.h"

namespace graspit_ros_planning
{


class RosGraspitInterface : public Plugin
{
private:
  ros::NodeHandle *root_nh_;

  ros::NodeHandle *priv_nh_;


  db_planner::DatabaseManager *db_mgr_;

  ros::ServiceServer load_model_srv_;

  ros::ServiceServer load_obstacle_srv_;

  ros::ServiceServer clear_bodies_srv_;

  ros::ServiceServer simulate_scan_srv_;

  ros::ServiceServer test_grasp_srv_;

  ros::ServiceServer grasp_planning_srv_;

  ros::ServiceServer generate_grasp_srv_;

  ros::ServiceServer verify_grasp_srv_;

  ros::Publisher scan_publisher_;

  std::map<int, GraspitDBModel*> models_;

  Pr2Gripper2010 *gripper_;

  int default_grasp_test_type_;

  double default_energy_threshold_;


  GraspitDBModel* getModel(int model_id);

  bool loadGripper();

  // ------------------------- helper functions for grasp tests ---------------------------

  void gripperCollisionCheck(const Body *object, graspit_ros_planning_msgs::TestGrasp::Response &response);

  void computeEnergy(Body *object, graspit_ros_planning_msgs::TestGrasp::Response &response);

  // ---------------------------------- grasp tests ----------------------------------------

  void testGraspDirect(const manipulation_msgs::Grasp &grasp, GraspableBody *object,
                       graspit_ros_planning_msgs::TestGrasp::Response &response);

  void testGraspCompliant(const manipulation_msgs::Grasp &grasp, GraspableBody *object,
                          graspit_ros_planning_msgs::TestGrasp::Response &response);

  void testGraspReactive(const manipulation_msgs::Grasp &grasp, GraspableBody *object,
                         graspit_ros_planning_msgs::TestGrasp::Response &response);

  void testGraspRobustReactive(const manipulation_msgs::Grasp &grasp, GraspableBody *object,
                               graspit_ros_planning_msgs::TestGrasp::Response &response);

  // ---------------------------------- callbacks ----------------------------------------

  bool loadModelCB(graspit_ros_planning_msgs::LoadDatabaseModel::Request &request,
                   graspit_ros_planning_msgs::LoadDatabaseModel::Response &response);

  bool loadObstacleCB(graspit_ros_planning_msgs::LoadObstacle::Request &request,
                      graspit_ros_planning_msgs::LoadObstacle::Response &response);

  bool clearBodiesCB(graspit_ros_planning_msgs::ClearBodies::Request &request,
                     graspit_ros_planning_msgs::ClearBodies::Response &response);

  bool simulateScanCB(graspit_ros_planning_msgs::SimulateScan::Request &request,
                      graspit_ros_planning_msgs::SimulateScan::Response &response);

  bool testGraspCB(graspit_ros_planning_msgs::TestGrasp::Request &request,
                   graspit_ros_planning_msgs::TestGrasp::Response &response);

  bool graspPlanningCB(manipulation_msgs::GraspPlanning::Request &request,
                       manipulation_msgs::GraspPlanning::Response &response);

  bool generateGraspCB(graspit_ros_planning_msgs::GenerateGrasp::Request &request,
                       graspit_ros_planning_msgs::GenerateGrasp::Response &response);

  bool verifyGraspCB(graspit_ros_planning_msgs::VerifyGrasp::Request &request,
                     graspit_ros_planning_msgs::VerifyGrasp::Response &response);


public:
  RosGraspitInterface();
  ~RosGraspitInterface();
  virtual int init(int argc, char **argv);
  virtual int mainLoop();
};

}

#endif