#include <ros_graspit_interface.h>
Public Member Functions | |
virtual int | init (int argc, char **argv) |
Creates the node handles, advertises services, connects to the database. | |
virtual int | mainLoop () |
Simply calls ros::spinOnce() to process the ROS event loop. | |
RosGraspitInterface () | |
Inits ROS, but (for now) without passing any arguments. | |
~RosGraspitInterface () | |
Deletes the node handle and the db manager. | |
Private Member Functions | |
bool | clearBodiesCB (graspit_interface_msgs::ClearBodies::Request &request, graspit_interface_msgs::ClearBodies::Response &response) |
Callback for the clear bodies service. | |
void | computeEnergy (Body *object, graspit_interface_msgs::TestGrasp::Response &response) |
Performs the grasp test using the grasp energy function. | |
bool | generateGraspCB (graspit_interface_msgs::GenerateGrasp::Request &request, graspit_interface_msgs::GenerateGrasp::Response &response) |
Callback for the grasp generation service, for PR2 gripper. | |
GraspitDBModel * | getModel (int model_id) |
-------------------------- populating the graspit world ------------------------------ | |
bool | graspPlanningCB (object_manipulation_msgs::GraspPlanning::Request &request, object_manipulation_msgs::GraspPlanning::Response &response) |
Callback for the general grasp planning service. | |
void | gripperCollisionCheck (const Body *object, graspit_interface_msgs::TestGrasp::Response &response) |
Checks collisions between the gripper and the environment. | |
bool | loadGripper () |
Loads the gripper info from its file, if not already loaded. | |
bool | loadModelCB (graspit_interface_msgs::LoadDatabaseModel::Request &request, graspit_interface_msgs::LoadDatabaseModel::Response &response) |
Callback for the load model service. | |
bool | loadObstacleCB (graspit_interface_msgs::LoadObstacle::Request &request, graspit_interface_msgs::LoadObstacle::Response &response) |
Callback for the load obstacle service. | |
bool | simulateScanCB (graspit_interface_msgs::SimulateScan::Request &request, graspit_interface_msgs::SimulateScan::Response &response) |
Callback for the clear bodies service. | |
bool | testGraspCB (graspit_interface_msgs::TestGrasp::Request &request, graspit_interface_msgs::TestGrasp::Response &response) |
Callback for the detailed, graspit-specific test grasp service. | |
void | testGraspCompliant (const object_manipulation_msgs::Grasp &grasp, GraspableBody *object, graspit_interface_msgs::TestGrasp::Response &response) |
Tests a grasp using the compliant method. | |
void | testGraspDirect (const object_manipulation_msgs::Grasp &grasp, GraspableBody *object, graspit_interface_msgs::TestGrasp::Response &response) |
Tests a grasp using the direct method. | |
void | testGraspReactive (const object_manipulation_msgs::Grasp &grasp, GraspableBody *object, graspit_interface_msgs::TestGrasp::Response &response) |
Tests a grasp using the direct method. | |
void | testGraspRobustReactive (const object_manipulation_msgs::Grasp &grasp, GraspableBody *object, graspit_interface_msgs::TestGrasp::Response &response) |
Tests a grasp using the direct method. | |
bool | verifyGraspCB (graspit_interface_msgs::VerifyGrasp::Request &request, graspit_interface_msgs::VerifyGrasp::Response &response) |
Callback for the grasp verifying service, for PR2 gripper. | |
Private Attributes | |
ros::ServiceServer | clear_bodies_srv_ |
Server for the clear bodies service. | |
db_planner::DatabaseManager * | db_mgr_ |
A GraspIt database manager used for talking to the database. | |
double | default_energy_threshold_ |
Used for converting energy values fo probabilities; soon to be replaced. | |
int | default_grasp_test_type_ |
Used for general GrapsPlanning calls; should be a value from graspit_interface_msgs::TestGrasp. | |
ros::ServiceServer | generate_grasp_srv_ |
Server for the generate grasp service. | |
ros::ServiceServer | grasp_planning_srv_ |
Server for the grasp planning service. | |
Pr2Gripper2010 * | gripper_ |
A GraspIt instance of the PR2 gripper. | |
ros::ServiceServer | load_model_srv_ |
Server for the load model service. | |
ros::ServiceServer | load_obstacle_srv_ |
Server for the load obstacle service. | |
std::map< int, GraspitDBModel * > | models_ |
Keeps track of all models already retrieved from the database, mapped by their scaled model id. | |
ros::NodeHandle * | priv_nh_ |
Node handle in the private namespace. | |
ros::NodeHandle * | root_nh_ |
Node handle in the root namespace. | |
ros::Publisher | scan_publisher_ |
Publisher for simulated scans. | |
ros::ServiceServer | simulate_scan_srv_ |
Server for the scan simulation service. | |
ros::ServiceServer | test_grasp_srv_ |
Server for the test grasp service. | |
ros::ServiceServer | verify_grasp_srv_ |
Server for the grasp collision checking service. |
Definition at line 66 of file ros_graspit_interface.h.
graspit_interface::RosGraspitInterface::RosGraspitInterface | ( | ) |
Inits ROS, but (for now) without passing any arguments.
Definition at line 158 of file ros_graspit_interface.cpp.
graspit_interface::RosGraspitInterface::~RosGraspitInterface | ( | ) |
Deletes the node handle and the db manager.
Definition at line 164 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::clearBodiesCB | ( | graspit_interface_msgs::ClearBodies::Request & | request, | |
graspit_interface_msgs::ClearBodies::Response & | response | |||
) | [private] |
Callback for the clear bodies service.
Definition at line 339 of file ros_graspit_interface.cpp.
void graspit_interface::RosGraspitInterface::computeEnergy | ( | Body * | object, | |
graspit_interface_msgs::TestGrasp::Response & | response | |||
) | [private] |
Performs the grasp test using the grasp energy function.
Definition at line 505 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::generateGraspCB | ( | graspit_interface_msgs::GenerateGrasp::Request & | request, | |
graspit_interface_msgs::GenerateGrasp::Response & | response | |||
) | [private] |
Callback for the grasp generation service, for PR2 gripper.
Definition at line 679 of file ros_graspit_interface.cpp.
GraspitDBModel * graspit_interface::RosGraspitInterface::getModel | ( | int | model_id | ) | [private] |
-------------------------- populating the graspit world ------------------------------
Will load a model, or retrieve it from the map of previously loaded models
If the model has laready been loaded, it will retrieve it from the list of models. If not, it will load it, add it to the list, and return it. Returns NULL only if the model could not be loaded from the database.
Definition at line 246 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::graspPlanningCB | ( | object_manipulation_msgs::GraspPlanning::Request & | request, | |
object_manipulation_msgs::GraspPlanning::Response & | response | |||
) | [private] |
Callback for the general grasp planning service.
Definition at line 1177 of file ros_graspit_interface.cpp.
void graspit_interface::RosGraspitInterface::gripperCollisionCheck | ( | const Body * | object, | |
graspit_interface_msgs::TestGrasp::Response & | response | |||
) | [private] |
Checks collisions between the gripper and the environment.
Definition at line 475 of file ros_graspit_interface.cpp.
int graspit_interface::RosGraspitInterface::init | ( | int | argc, | |
char ** | argv | |||
) | [virtual] |
Creates the node handles, advertises services, connects to the database.
Definition at line 173 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::loadGripper | ( | ) | [private] |
Loads the gripper info from its file, if not already loaded.
Definition at line 264 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::loadModelCB | ( | graspit_interface_msgs::LoadDatabaseModel::Request & | request, | |
graspit_interface_msgs::LoadDatabaseModel::Response & | response | |||
) | [private] |
Callback for the load model service.
Definition at line 283 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::loadObstacleCB | ( | graspit_interface_msgs::LoadObstacle::Request & | request, | |
graspit_interface_msgs::LoadObstacle::Response & | response | |||
) | [private] |
Callback for the load obstacle service.
Definition at line 376 of file ros_graspit_interface.cpp.
int graspit_interface::RosGraspitInterface::mainLoop | ( | ) | [virtual] |
Simply calls ros::spinOnce() to process the ROS event loop.
Definition at line 236 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::simulateScanCB | ( | graspit_interface_msgs::SimulateScan::Request & | request, | |
graspit_interface_msgs::SimulateScan::Response & | response | |||
) | [private] |
Callback for the clear bodies service.
Definition at line 395 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::testGraspCB | ( | graspit_interface_msgs::TestGrasp::Request & | request, | |
graspit_interface_msgs::TestGrasp::Response & | response | |||
) | [private] |
Callback for the detailed, graspit-specific test grasp service.
Definition at line 1129 of file ros_graspit_interface.cpp.
void graspit_interface::RosGraspitInterface::testGraspCompliant | ( | const object_manipulation_msgs::Grasp & | grasp, | |
GraspableBody * | object, | |||
graspit_interface_msgs::TestGrasp::Response & | response | |||
) | [private] |
Tests a grasp using the compliant method.
Definition at line 987 of file ros_graspit_interface.cpp.
void graspit_interface::RosGraspitInterface::testGraspDirect | ( | const object_manipulation_msgs::Grasp & | grasp, | |
GraspableBody * | object, | |||
graspit_interface_msgs::TestGrasp::Response & | response | |||
) | [private] |
Tests a grasp using the direct method.
Definition at line 967 of file ros_graspit_interface.cpp.
void graspit_interface::RosGraspitInterface::testGraspReactive | ( | const object_manipulation_msgs::Grasp & | grasp, | |
GraspableBody * | object, | |||
graspit_interface_msgs::TestGrasp::Response & | response | |||
) | [private] |
Tests a grasp using the direct method.
Definition at line 1008 of file ros_graspit_interface.cpp.
void graspit_interface::RosGraspitInterface::testGraspRobustReactive | ( | const object_manipulation_msgs::Grasp & | grasp, | |
GraspableBody * | object, | |||
graspit_interface_msgs::TestGrasp::Response & | response | |||
) | [private] |
Tests a grasp using the direct method.
Definition at line 1035 of file ros_graspit_interface.cpp.
bool graspit_interface::RosGraspitInterface::verifyGraspCB | ( | graspit_interface_msgs::VerifyGrasp::Request & | request, | |
graspit_interface_msgs::VerifyGrasp::Response & | response | |||
) | [private] |
Callback for the grasp verifying service, for PR2 gripper.
Definition at line 529 of file ros_graspit_interface.cpp.
ros::ServiceServer graspit_interface::RosGraspitInterface::clear_bodies_srv_ [private] |
Server for the clear bodies service.
Definition at line 84 of file ros_graspit_interface.h.
db_planner::DatabaseManager* graspit_interface::RosGraspitInterface::db_mgr_ [private] |
A GraspIt database manager used for talking to the database.
An interesting point is that here we also have to option of using a ROS interface to the database; pros and cons should be considered in the future.
Definition at line 75 of file ros_graspit_interface.h.
double graspit_interface::RosGraspitInterface::default_energy_threshold_ [private] |
Used for converting energy values fo probabilities; soon to be replaced.
Definition at line 116 of file ros_graspit_interface.h.
Used for general GrapsPlanning calls; should be a value from graspit_interface_msgs::TestGrasp.
Definition at line 113 of file ros_graspit_interface.h.
ros::ServiceServer graspit_interface::RosGraspitInterface::generate_grasp_srv_ [private] |
Server for the generate grasp service.
Definition at line 96 of file ros_graspit_interface.h.
ros::ServiceServer graspit_interface::RosGraspitInterface::grasp_planning_srv_ [private] |
Server for the grasp planning service.
Definition at line 93 of file ros_graspit_interface.h.
Pr2Gripper2010* graspit_interface::RosGraspitInterface::gripper_ [private] |
A GraspIt instance of the PR2 gripper.
Definition at line 110 of file ros_graspit_interface.h.
ros::ServiceServer graspit_interface::RosGraspitInterface::load_model_srv_ [private] |
Server for the load model service.
Definition at line 78 of file ros_graspit_interface.h.
ros::ServiceServer graspit_interface::RosGraspitInterface::load_obstacle_srv_ [private] |
Server for the load obstacle service.
Definition at line 81 of file ros_graspit_interface.h.
std::map<int, GraspitDBModel*> graspit_interface::RosGraspitInterface::models_ [private] |
Keeps track of all models already retrieved from the database, mapped by their scaled model id.
Definition at line 107 of file ros_graspit_interface.h.
ros::NodeHandle* graspit_interface::RosGraspitInterface::priv_nh_ [private] |
Node handle in the private namespace.
Definition at line 70 of file ros_graspit_interface.h.
ros::NodeHandle* graspit_interface::RosGraspitInterface::root_nh_ [private] |
Node handle in the root namespace.
Definition at line 67 of file ros_graspit_interface.h.
ros::Publisher graspit_interface::RosGraspitInterface::scan_publisher_ [private] |
Publisher for simulated scans.
Definition at line 104 of file ros_graspit_interface.h.
ros::ServiceServer graspit_interface::RosGraspitInterface::simulate_scan_srv_ [private] |
Server for the scan simulation service.
Definition at line 87 of file ros_graspit_interface.h.
ros::ServiceServer graspit_interface::RosGraspitInterface::test_grasp_srv_ [private] |
Server for the test grasp service.
Definition at line 90 of file ros_graspit_interface.h.
ros::ServiceServer graspit_interface::RosGraspitInterface::verify_grasp_srv_ [private] |
Server for the grasp collision checking service.
Definition at line 99 of file ros_graspit_interface.h.