objects_database_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Author(s): Matei Ciocarlie
00036 
00039 
00040 #include <vector>
00041 #include <boost/shared_ptr.hpp>
00042 
00043 #include <ros/ros.h>
00044 
00045 #include <actionlib/server/simple_action_server.h>
00046 
00047 #include <tf/transform_datatypes.h>
00048 #include <tf/transform_listener.h>
00049 
00050 #include <object_manipulation_msgs/GraspPlanning.h>
00051 #include <object_manipulation_msgs/GraspPlanningAction.h>
00052 
00053 #include <household_objects_database_msgs/GetModelList.h>
00054 #include <household_objects_database_msgs/GetModelMesh.h>
00055 #include <household_objects_database_msgs/GetModelDescription.h>
00056 #include <household_objects_database_msgs/GetModelScans.h>
00057 #include <household_objects_database_msgs/DatabaseScan.h>
00058 #include <household_objects_database_msgs/SaveScan.h>
00059 #include <household_objects_database_msgs/TranslateRecognitionId.h>
00060 
00061 #include "household_objects_database/objects_database.h"
00062 
00063 const std::string GET_MODELS_SERVICE_NAME = "get_model_list";
00064 const std::string GET_MESH_SERVICE_NAME = "get_model_mesh";
00065 const std::string GET_DESCRIPTION_SERVICE_NAME = "get_model_description";
00066 const std::string GRASP_PLANNING_SERVICE_NAME = "database_grasp_planning";
00067 const std::string GET_SCANS_SERVICE_NAME = "get_model_scans";
00068 const std::string SAVE_SCAN_SERVICE_NAME = "save_model_scan";
00069 const std::string TRANSLATE_ID_SERVICE_NAME = "translate_id";
00070 const std::string GRASP_PLANNING_ACTION_NAME = "database_grasp_planning";
00071 
00072 using namespace household_objects_database_msgs;
00073 using namespace household_objects_database;
00074 using namespace object_manipulation_msgs;
00075 
00077 
00078 class HandDescription
00079 {
00080  private:
00082   ros::NodeHandle root_nh_;
00083 
00084   inline std::string getStringParam(std::string name)
00085   {
00086     std::string value;
00087     if (!root_nh_.getParamCached(name, value))
00088     {
00089       ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
00090     }
00091     //ROS_INFO_STREAM("Hand description param " << name << " resolved to " << value);
00092     return value;
00093   }
00094 
00095   inline std::vector<std::string> getVectorParam(std::string name)
00096   {
00097     std::vector<std::string> values;
00098     XmlRpc::XmlRpcValue list;
00099     if (!root_nh_.getParamCached(name, list)) 
00100     {
00101       ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
00102     }
00103     if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00104     {
00105       ROS_ERROR("Hand description: bad parameter %s", name.c_str());
00106     }
00107     //ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:");
00108     for (int32_t i=0; i<list.size(); i++)
00109     {
00110       if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
00111       {
00112         ROS_ERROR("Hand description: bad parameter %s", name.c_str());
00113       }
00114       values.push_back( static_cast<std::string>(list[i]) );
00115       //ROS_INFO_STREAM("  " << values.back());
00116     }
00117     return values;      
00118   }
00119 
00120  public:
00121  HandDescription() : root_nh_("~") {}
00122   
00123   inline std::string handDatabaseName(std::string arm_name)
00124   {
00125     return getStringParam("/hand_description/" + arm_name + "/hand_database_name");
00126   }
00127   
00128   inline std::vector<std::string> handJointNames(std::string arm_name)
00129   {
00130     return getVectorParam("/hand_description/" + arm_name + "/hand_joints");
00131   }
00132   
00133 };
00134 
00135 bool greaterScaledQuality(const boost::shared_ptr<DatabaseGrasp> &grasp1, 
00136                           const boost::shared_ptr<DatabaseGrasp> &grasp2)
00137 {
00138   if (grasp1->scaled_quality_.data() > grasp2->scaled_quality_.data()) return true;
00139   return false;
00140 }
00141 
00143 
00145 class ObjectsDatabaseNode
00146 {
00147 private:
00149   ros::NodeHandle priv_nh_;
00150 
00152   ros::NodeHandle root_nh_;
00153 
00155   ros::ServiceServer get_models_srv_;
00156 
00158   ros::ServiceServer get_mesh_srv_;
00159 
00161   ros::ServiceServer get_description_srv_;
00162 
00164   ros::ServiceServer grasp_planning_srv_;
00165 
00167   actionlib::SimpleActionServer<object_manipulation_msgs::GraspPlanningAction> *grasp_planning_server_;  
00168 
00170   ros::ServiceServer get_scans_srv_;
00171 
00173   ros::ServiceServer save_scan_srv_;
00174 
00176   ros::ServiceServer translate_id_srv_;
00177 
00179   ObjectsDatabase *database_;
00180 
00182   tf::TransformListener listener_;
00183 
00185 
00186   std::string grasp_ordering_method_;
00187 
00188   bool translateIdCB(TranslateRecognitionId::Request &request, TranslateRecognitionId::Response &response)
00189   {
00190     std::vector<boost::shared_ptr<DatabaseScaledModel> > models;
00191     if (!database_)
00192     {
00193       ROS_ERROR("Translate is service: database not connected");
00194       response.result = response.DATABASE_ERROR;
00195       return true;
00196     }
00197     if (!database_->getScaledModelsByRecognitionId(models, request.recognition_id))
00198     {
00199       ROS_ERROR("Translate is service: query failed");
00200       response.result = response.DATABASE_ERROR;
00201       return true;
00202     }
00203     if (models.empty())
00204     {
00205       ROS_ERROR("Translate is service: recognition id %s not found", request.recognition_id.c_str());
00206       response.result = response.ID_NOT_FOUND;
00207       return true;
00208     }
00209     response.household_objects_id = models[0]->id_.data();
00210     if (models.size() > 1) ROS_WARN("Multiple matches found for recognition id %s. Returning the first one.",
00211                                     request.recognition_id.c_str());
00212     response.result = response.SUCCESS;
00213     return true;
00214   }
00215 
00217   bool getModelsCB(GetModelList::Request &request, GetModelList::Response &response)
00218   {
00219     if (!database_)
00220     {
00221       response.return_code.code = response.return_code.DATABASE_NOT_CONNECTED;
00222       return true;
00223     }
00224     std::vector< boost::shared_ptr<DatabaseScaledModel> > models;
00225     if (!database_->getScaledModelsBySet(models, request.model_set))
00226     {
00227       response.return_code.code = response.return_code.DATABASE_QUERY_ERROR;
00228       return true;
00229     }
00230     for (size_t i=0; i<models.size(); i++)
00231     {
00232       response.model_ids.push_back( models[i]->id_.data() );
00233     }
00234     response.return_code.code = response.return_code.SUCCESS;
00235     return true;
00236   }
00237 
00239   bool getMeshCB(GetModelMesh::Request &request, GetModelMesh::Response &response)
00240   {
00241     if (!database_)
00242     {
00243       response.return_code.code = response.return_code.DATABASE_NOT_CONNECTED;
00244       return true;
00245     }
00246     if ( !database_->getScaledModelMesh(request.model_id, response.mesh) )
00247     {
00248       response.return_code.code = response.return_code.DATABASE_QUERY_ERROR;
00249       return true;
00250     }
00251     response.return_code.code = response.return_code.SUCCESS;
00252     return true;
00253   }
00254 
00256   bool getDescriptionCB(GetModelDescription::Request &request, GetModelDescription::Response &response)
00257   {
00258     if (!database_)
00259     {
00260       response.return_code.code = response.return_code.DATABASE_NOT_CONNECTED;
00261       return true;
00262     }
00263     std::vector< boost::shared_ptr<DatabaseScaledModel> > models;
00264     
00265     std::stringstream id;
00266     id << request.model_id;
00267     std::string where_clause("scaled_model_id=" + id.str());
00268     if (!database_->getList(models, where_clause) || models.size() != 1)
00269     {
00270       response.return_code.code = response.return_code.DATABASE_QUERY_ERROR;
00271       return true;
00272     }
00273     response.tags = models[0]->tags_.data();
00274     response.name = models[0]->model_.data();
00275     response.maker = models[0]->maker_.data();
00276     response.return_code.code = response.return_code.SUCCESS;
00277     return true;
00278   }
00279 
00280   bool getScansCB(GetModelScans::Request &request, GetModelScans::Response &response)
00281   {
00282     if (!database_)
00283     {
00284       ROS_ERROR("GetModelScan: database not connected");
00285       response.return_code.code = DatabaseReturnCode::DATABASE_NOT_CONNECTED;
00286       return true;
00287     }
00288 
00289     database_->getModelScans(request.model_id, request.scan_source,response.matching_scans);
00290     response.return_code.code = DatabaseReturnCode::SUCCESS;
00291     return true;
00292   }
00293 
00294   bool saveScanCB(SaveScan::Request &request, SaveScan::Response &response)
00295   {
00296     if (!database_)
00297     {
00298       ROS_ERROR("SaveScan: database not connected");
00299       response.return_code.code = DatabaseReturnCode::DATABASE_NOT_CONNECTED;
00300       return true;
00301     }
00302     household_objects_database::DatabaseScan scan;
00303     scan.frame_id_.get() = request.ground_truth_pose.header.frame_id;
00304     scan.cloud_topic_.get() = request.cloud_topic;
00305     scan.object_pose_.get().pose_ = request.ground_truth_pose.pose;
00306     scan.scaled_model_id_.get() = request.scaled_model_id;
00307     scan.scan_bagfile_location_.get() = request.bagfile_location;
00308     scan.scan_source_.get() = request.scan_source;
00309     database_->insertIntoDatabase(&scan);
00310     response.return_code.code = DatabaseReturnCode::SUCCESS;
00311     return true;
00312   }
00313 
00314   geometry_msgs::Pose multiplyPoses(const geometry_msgs::Pose &p1, 
00315                                     const geometry_msgs::Pose &p2)
00316   {
00317     tf::Transform t1;
00318     tf::poseMsgToTF(p1, t1);
00319     tf::Transform t2;
00320     tf::poseMsgToTF(p2, t2);
00321     t2 = t1 * t2;        
00322     geometry_msgs::Pose out_pose;
00323     tf::poseTFToMsg(t2, out_pose);
00324     return out_pose;
00325   }
00326 
00327   //retrieves all grasps from the database for a given target
00328   bool getGrasps(const GraspableObject &target, const std::string &arm_name, 
00329                  std::vector<Grasp> &grasps, GraspPlanningErrorCode &error_code)
00330   {
00331     if (!database_)
00332     {
00333       ROS_ERROR("Database grasp planning: database not connected");
00334       error_code.value = error_code.OTHER_ERROR;
00335       return false;
00336     }
00337 
00338     if (target.potential_models.empty())
00339     {
00340       ROS_ERROR("Database grasp planning: no potential model information in grasp planning target");
00341       error_code.value = error_code.OTHER_ERROR;
00342       return false;      
00343     }
00344 
00345     if (target.potential_models.size() > 1)
00346     {
00347       ROS_WARN("Database grasp planning: target has more than one potential models. "
00348                "Returning grasps for first model only");
00349     }
00350 
00351     HandDescription hd;
00352     int model_id = target.potential_models[0].model_id;
00353     std::string hand_id = hd.handDatabaseName(arm_name);
00354     
00355     //retrieve the raw grasps from the database
00356     std::vector< boost::shared_ptr<DatabaseGrasp> > db_grasps;
00357     if (!database_->getClusterRepGrasps(model_id, hand_id, db_grasps))
00358     {
00359       ROS_ERROR("Database grasp planning: database query error");
00360       error_code.value = error_code.OTHER_ERROR;
00361       return false;
00362     }
00363     ROS_INFO("Database object node: retrieved %u grasps from database", (unsigned int)db_grasps.size());
00364 
00365     //order grasps based on request
00366     if (grasp_ordering_method_ == "random") 
00367     {
00368       ROS_INFO("Randomizing grasps");
00369       std::random_shuffle(db_grasps.begin(), db_grasps.end());
00370     }
00371     else if (grasp_ordering_method_ == "quality")
00372     {
00373       ROS_INFO("Sorting grasps by scaled quality");
00374       std::sort(db_grasps.begin(), db_grasps.end(), greaterScaledQuality);
00375     }
00376     else
00377     {
00378       ROS_WARN("Unknown grasp ordering method requested -- randomizing grasp order");
00379       std::random_shuffle(db_grasps.begin(), db_grasps.end());
00380     }
00381 
00382     //convert to the Grasp data type
00383     std::vector< boost::shared_ptr<DatabaseGrasp> >::iterator it;
00384     for (it = db_grasps.begin(); it != db_grasps.end(); it++)
00385     {
00386       ROS_ASSERT( (*it)->final_grasp_posture_.get().joint_angles_.size() == 
00387                   (*it)->pre_grasp_posture_.get().joint_angles_.size() );
00388       Grasp grasp;
00389       std::vector<std::string> joint_names = hd.handJointNames(arm_name);
00390 
00391       if (hand_id != "WILLOW_GRIPPER_2010")
00392       {
00393         //check that the number of joints in the ROS description of this hand
00394         //matches the number of values we have in the database
00395         if (joint_names.size() != (*it)->final_grasp_posture_.get().joint_angles_.size())
00396         {
00397           ROS_ERROR("Database grasp specification does not match ROS description of hand. "
00398                     "Hand is expected to have %d joints, but database grasp specifies %d values", 
00399                     (int)joint_names.size(), (int)(*it)->final_grasp_posture_.get().joint_angles_.size());
00400           continue;
00401         }
00402         //for now we silently assume that the order of the joints in the ROS description of
00403         //the hand is the same as in the database description
00404         grasp.pre_grasp_posture.name = joint_names;
00405         grasp.grasp_posture.name = joint_names;
00406         grasp.pre_grasp_posture.position = (*it)->pre_grasp_posture_.get().joint_angles_;
00407         grasp.grasp_posture.position = (*it)->final_grasp_posture_.get().joint_angles_; 
00408       }
00409       else
00410       {
00411         //unfortunately we have to hack this, as the grasp is really defined by a single
00412         //DOF, but the urdf for the PR2 gripper is not well set up to do that
00413         if ( joint_names.size() != 4 || (*it)->final_grasp_posture_.get().joint_angles_.size() != 1)
00414         {
00415           ROS_ERROR("PR2 gripper specs and database grasp specs do not match expected values");
00416           continue;
00417         }
00418         grasp.pre_grasp_posture.name = joint_names;
00419         grasp.grasp_posture.name = joint_names;
00420         //replicate the single value from the database 4 times
00421         grasp.pre_grasp_posture.position.resize( joint_names.size(), 
00422                                                  (*it)->pre_grasp_posture_.get().joint_angles_.at(0));
00423         grasp.grasp_posture.position.resize( joint_names.size(), 
00424                                              (*it)->final_grasp_posture_.get().joint_angles_.at(0));
00425       }
00426       //for now the effort is not in the database so we hard-code it in here
00427       //this will change at some point
00428       grasp.grasp_posture.effort.resize(joint_names.size(), 50);
00429       grasp.pre_grasp_posture.effort.resize(joint_names.size(), 100);
00430       //min and desired approach distances are the same for all grasps
00431       grasp.desired_approach_distance = 0.10;
00432       grasp.min_approach_distance = 0.05;
00433       //the pose of the grasp
00434       grasp.grasp_pose = (*it)->final_grasp_pose_.get().pose_;
00435       //convert it to the frame of the detection
00436       grasp.grasp_pose = multiplyPoses(target.potential_models[0].pose.pose, grasp.grasp_pose);
00437       //and then finally to the reference frame of the object
00438       if (target.potential_models[0].pose.header.frame_id !=
00439           target.reference_frame_id)
00440       {
00441         tf::StampedTransform ref_trans;
00442         try
00443         {
00444           listener_.lookupTransform(target.reference_frame_id,
00445                                     target.potential_models[0].pose.header.frame_id,                    
00446                                     ros::Time(0), ref_trans);
00447         }
00448         catch (tf::TransformException ex)
00449         {
00450           ROS_ERROR("Grasp planner: failed to get transform from %s to %s; exception: %s", 
00451                     target.reference_frame_id.c_str(), 
00452                     target.potential_models[0].pose.header.frame_id.c_str(), ex.what());
00453           error_code.value = error_code.OTHER_ERROR;
00454           return false;      
00455         }        
00456         geometry_msgs::Pose ref_pose;
00457         tf::poseTFToMsg(ref_trans, ref_pose);
00458         grasp.grasp_pose = multiplyPoses(ref_pose, grasp.grasp_pose);
00459       }
00460       //stick the scaled quality into the success_probability field
00461       grasp.success_probability = (*it)->scaled_quality_.get();
00462 
00463       //insert the new grasp in the list
00464       grasps.push_back(grasp);
00465     }
00466 
00467     ROS_INFO("Database grasp planner: returning %u grasps", (unsigned int)grasps.size());
00468     error_code.value = error_code.SUCCESS;
00469     return true;
00470   }
00471 
00473   bool graspPlanningCB(GraspPlanning::Request &request, GraspPlanning::Response &response)
00474   {
00475     getGrasps(request.target, request.arm_name, response.grasps, response.error_code);
00476     return true;
00477   }
00478 
00479   void graspPlanningActionCB(const object_manipulation_msgs::GraspPlanningGoalConstPtr &goal)
00480   {
00481     std::vector<Grasp> grasps;
00482     GraspPlanningErrorCode error_code;
00483     GraspPlanningResult result;
00484     GraspPlanningFeedback feedback;
00485     bool success = getGrasps(goal->target, goal->arm_name, grasps, error_code);
00486     /*
00487     for (size_t i=0; i<grasps.size(); i++)
00488     {
00489       if (grasp_planning_server_->isPreemptRequested()) 
00490       {
00491         grasp_planning_server_->setAborted(result);
00492         return;
00493       }
00494       feedback.grasps.push_back(grasps[i]);
00495       if ( (i+1)%10==0)
00496       {
00497         grasp_planning_server_->publishFeedback(feedback);
00498         ros::Duration(10.0).sleep();
00499       }
00500     }
00501     */
00502     result.grasps = grasps;
00503     result.error_code = error_code;
00504     if (!success) 
00505     {
00506       grasp_planning_server_->setAborted(result);
00507       return;
00508     }
00509     grasp_planning_server_->setSucceeded(result);
00510   }
00511 
00512 public:
00513   ObjectsDatabaseNode() : priv_nh_("~"), root_nh_("")
00514   {
00515     //initialize database connection
00516     std::string database_host, database_port, database_user, database_pass, database_name;
00517     root_nh_.param<std::string>("/household_objects_database/database_host", database_host, "");
00518     int port_int;
00519     root_nh_.param<int>("/household_objects_database/database_port", port_int, -1);
00520     std::stringstream ss; ss << port_int; database_port = ss.str();
00521     root_nh_.param<std::string>("/household_objects_database/database_user", database_user, "");
00522     root_nh_.param<std::string>("/household_objects_database/database_pass", database_pass, "");
00523     root_nh_.param<std::string>("/household_objects_database/database_name", database_name, "");
00524     database_ = new ObjectsDatabase(database_host, database_port, database_user, database_pass, database_name);
00525     if (!database_->isConnected())
00526     {
00527       ROS_ERROR("ObjectsDatabaseNode: failed to open model database on host "
00528                 "%s, port %s, user %s with password %s, database %s. Unable to do grasp "
00529                 "planning on database recognized objects. Exiting.",
00530                 database_host.c_str(), database_port.c_str(), 
00531                 database_user.c_str(), database_pass.c_str(), database_name.c_str());
00532       delete database_; database_ = NULL;
00533     }
00534 
00535     //advertise services
00536     get_models_srv_ = priv_nh_.advertiseService(GET_MODELS_SERVICE_NAME, &ObjectsDatabaseNode::getModelsCB, this);    
00537     get_mesh_srv_ = priv_nh_.advertiseService(GET_MESH_SERVICE_NAME, &ObjectsDatabaseNode::getMeshCB, this);    
00538     get_description_srv_ = priv_nh_.advertiseService(GET_DESCRIPTION_SERVICE_NAME, 
00539                                                      &ObjectsDatabaseNode::getDescriptionCB, this);    
00540     grasp_planning_srv_ = priv_nh_.advertiseService(GRASP_PLANNING_SERVICE_NAME, 
00541                                                     &ObjectsDatabaseNode::graspPlanningCB, this);
00542 
00543     get_scans_srv_ = priv_nh_.advertiseService(GET_SCANS_SERVICE_NAME,
00544                                                &ObjectsDatabaseNode::getScansCB, this);
00545     save_scan_srv_ = priv_nh_.advertiseService(SAVE_SCAN_SERVICE_NAME,
00546                                                &ObjectsDatabaseNode::saveScanCB, this);
00547     translate_id_srv_ = priv_nh_.advertiseService(TRANSLATE_ID_SERVICE_NAME, 
00548                                                   &ObjectsDatabaseNode::translateIdCB, this);
00549 
00550     priv_nh_.param<std::string>("grasp_ordering_method", grasp_ordering_method_, "random");
00551 
00552     grasp_planning_server_ = new actionlib::SimpleActionServer<object_manipulation_msgs::GraspPlanningAction>
00553       (root_nh_, GRASP_PLANNING_ACTION_NAME, 
00554        boost::bind(&ObjectsDatabaseNode::graspPlanningActionCB, this, _1), false);
00555     grasp_planning_server_->start();
00556   }
00557 
00558   ~ObjectsDatabaseNode()
00559   {
00560     delete database_;
00561     delete grasp_planning_server_;
00562   }
00563 };
00564 
00565 int main(int argc, char **argv)
00566 {
00567   ros::init(argc, argv, "objects_database_node");
00568   ObjectsDatabaseNode node;
00569   ros::spin();
00570   return 0;  
00571 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


household_objects_database
Author(s): Matei Ciocarlie, except for source files individually marked otherwise
autogenerated on Fri Jan 25 2013 15:02:09