ModelGenerator.cpp
Go to the documentation of this file.
00001 
00013 // RAIL Recognition
00014 #include "rail_recognition/ModelGenerator.h"
00015 #include "rail_recognition/PointCloudMetrics.h"
00016 
00017 // ROS
00018 #include <geometry_msgs/PoseArray.h>
00019 #include <pcl_ros/point_cloud.h>
00020 
00021 using namespace std;
00022 using namespace rail::pick_and_place;
00023 
00024 ModelGenerator::ModelGenerator()
00025     : private_node_("~"),
00026       as_(private_node_, "generate_models", boost::bind(&ModelGenerator::generateModelsCallback, this, _1), false)
00027 {
00028   // set defaults
00029   debug_ = DEFAULT_DEBUG;
00030   int port = graspdb::Client::DEFAULT_PORT;
00031   string host("127.0.0.1");
00032   string user("ros");
00033   string password("");
00034   string db("graspdb");
00035 
00036   // grab any parameters we need
00037   private_node_.getParam("debug", debug_);
00038   node_.getParam("/graspdb/host", host);
00039   node_.getParam("/graspdb/port", port);
00040   node_.getParam("/graspdb/user", user);
00041   node_.getParam("/graspdb/password", password);
00042   node_.getParam("/graspdb/db", db);
00043 
00044   // connect to the grasp database
00045   graspdb_ = new graspdb::Client(host, port, user, password, db);
00046   okay_ = graspdb_->connect();
00047 
00048   // setup a debug publisher if we need it
00049   if (debug_)
00050   {
00051     debug_pc_pub_ = private_node_.advertise<pcl::PointCloud<pcl::PointXYZRGB> >("debug_pc", 1, true);
00052     debug_poses_pub_ = private_node_.advertise<geometry_msgs::PoseArray>("debug_poses", 1, true);
00053   }
00054 
00055   if (okay_)
00056   {
00057     ROS_INFO("Model Generator Successfully Initialized");
00058   }
00059 
00060   as_.start();
00061 }
00062 
00063 ModelGenerator::~ModelGenerator()
00064 {
00065   // cleanup
00066   as_.shutdown();
00067   graspdb_->disconnect();
00068   delete graspdb_;
00069 }
00070 
00071 bool ModelGenerator::okay() const
00072 {
00073   return okay_;
00074 }
00075 
00076 
00077 void ModelGenerator::generateModelsCallback(const rail_pick_and_place_msgs::GenerateModelsGoalConstPtr &goal)
00078 {
00079   ROS_INFO("Model generation request received.");
00080 
00081   rail_pick_and_place_msgs::GenerateModelsResult result;
00082   rail_pick_and_place_msgs::GenerateModelsFeedback feedback;
00083 
00084   // load each grasp demonstration
00085   feedback.message = "Loading grasp demonstrations...";
00086   as_.publishFeedback(feedback);
00087   vector<PCLGraspModel> grasp_models;
00088   for (size_t i = 0; i < goal->grasp_demonstration_ids.size(); i++)
00089   {
00090     graspdb::GraspDemonstration demonstration;
00091     if (graspdb_->loadGraspDemonstration(goal->grasp_demonstration_ids[i], demonstration))
00092     {
00093       // translate the demonstration into a grasp model
00094       graspdb::GraspModel model;
00095       model.setPointCloud(demonstration.getPointCloud());
00096       model.setObjectName(demonstration.getObjectName());
00097       graspdb::Grasp grasp;
00098       grasp.setGraspPose(demonstration.getGraspPose());
00099       grasp.setEefFrameID(demonstration.getEefFrameID());
00100       model.addGrasp(grasp);
00101 
00102       // convert to a PCL version of the grasp model
00103       PCLGraspModel pcl_grasp_model(model);
00104       grasp_models.push_back(pcl_grasp_model);
00105     } else
00106     {
00107       ROS_WARN("Could not load grasp demonstration with ID %d.", goal->grasp_demonstration_ids[i]);
00108     }
00109   }
00110 
00111   // load each existing model
00112   feedback.message = "Loading grasp models...";
00113   as_.publishFeedback(feedback);
00114   for (size_t i = 0; i < goal->grasp_model_ids.size(); i++)
00115   {
00116     graspdb::GraspModel model;
00117     if (graspdb_->loadGraspModel(goal->grasp_model_ids[i], model))
00118     {
00119       // convert to a PCL version of the grasp model
00120       PCLGraspModel pcl_grasp_model(model);
00121       grasp_models.push_back(pcl_grasp_model);
00122     } else
00123     {
00124       ROS_WARN("Could not load grasp model with ID %d.", goal->grasp_model_ids[i]);
00125     }
00126   }
00127 
00128   // generate and store the models
00129   feedback.message = "Registering models...";
00130   as_.publishFeedback(feedback);
00131   this->generateAndStoreModels(grasp_models, goal->max_model_size, result.new_model_ids);
00132 
00133   // finished
00134   as_.setSucceeded(result, "Success!");
00135 }
00136 
00137 void ModelGenerator::generateAndStoreModels(vector<PCLGraspModel> &grasp_models, const int max_model_size,
00138                                             vector<uint32_t> &new_model_ids)
00139 {
00140   rail_pick_and_place_msgs::GenerateModelsFeedback feedback;
00141 
00142   // filter each point cloud, move to the origin, set unique IDs, and ensure they are flagged as original
00143   feedback.message = "Filtinering point clouds...";
00144   as_.publishFeedback(feedback);
00145   uint32_t id_counter = 0;
00146   for (size_t i = 0; i < grasp_models.size(); i++)
00147   {
00148     // filter the resulting PC
00149     point_cloud_metrics::filterPointCloudOutliers(grasp_models[i].getPCLPointCloud());
00150     point_cloud_metrics::transformToOrigin(grasp_models[i].getPCLPointCloud(), grasp_models[i].getGrasps());
00151     // set a unique ID
00152     grasp_models[i].setID(id_counter++);
00153     // flag as an original model
00154     grasp_models[i].setOriginal(true);
00155   }
00156 
00157   // create the initial pairings between all vertices
00158   vector<pair<uint32_t, uint32_t> > edges;
00159   for (size_t i = 0; i < grasp_models.size() - 1; i++)
00160   {
00161     for (size_t j = i + 1; j < grasp_models.size(); j++)
00162     {
00163       // add the pairing as an edge (use the larger as the first)
00164       pair<uint32_t, uint32_t> edge;
00165       if (grasp_models[i].getPCLPointCloud()->size() > grasp_models[j].getPCLPointCloud()->size())
00166       {
00167         edge.first = grasp_models[i].getID();
00168         edge.second = grasp_models[j].getID();
00169       } else
00170       {
00171         edge.first = grasp_models[j].getID();
00172         edge.second = grasp_models[i].getID();
00173       }
00174       edges.push_back(edge);
00175     }
00176   }
00177 
00178   // attempt to pair models
00179   feedback.message = "Searching graph for valid registrations...";
00180   as_.publishFeedback(feedback);
00181   ROS_INFO("%s", feedback.message.c_str());
00182   while (!edges.empty())
00183   {
00184     // randomly select the element to increase variability
00185     int rand_index = rand() % edges.size();
00186     pair<uint32_t, uint32_t> edge = edges[rand_index];
00187     edges.erase(edges.begin() + rand_index);
00188 
00189     // search for the base and target models
00190     size_t base_index, target_index;
00191     for (size_t i = 0; i < grasp_models.size(); i++)
00192     {
00193       if (grasp_models[i].getID() == edge.first)
00194       {
00195         base_index = i;
00196       } else if (grasp_models[i].getID() == edge.second)
00197       {
00198         target_index = i;
00199       }
00200     }
00201 
00202     // keep track of the base and the target models
00203     PCLGraspModel &base = grasp_models[base_index];
00204     PCLGraspModel &target = grasp_models[target_index];
00205 
00206     // used in feedback messages
00207     stringstream ss;
00208     ss << base.getID() << "-" << target.getID();
00209     string pair_str = ss.str();
00210 
00211     feedback.message = "Checking pair " + pair_str + "...";
00212     as_.publishFeedback(feedback);
00213 
00214     // check if the maximum size would be allowed
00215     if (base.getNumGrasps() + target.getNumGrasps() > max_model_size)
00216     {
00217       feedback.message = "Skipping pair " + pair_str + "as a merge would exceed the maximum model size.";
00218       as_.publishFeedback(feedback);
00219       ROS_WARN("%s", feedback.message.c_str());
00220     } else
00221     {
00222       // check if the registration passes
00223       PCLGraspModel result;
00224       if (this->registrationCheck(base, target, result))
00225       {
00226         feedback.message = "Registration match found for pair " + pair_str + ".";
00227         as_.publishFeedback(feedback);
00228         ROS_INFO("%s", feedback.message.c_str());
00229 
00230         // remove any edges containing these nodes
00231         for (int i = (int) edges.size() - 1; i >= 0; i--)
00232         {
00233           if (edges[i].first == base.getID() || edges[i].second == base.getID() || edges[i].first == target.getID()
00234               || edges[i].second == target.getID())
00235           {
00236             edges.erase(edges.begin() + i);
00237           }
00238         }
00239 
00240         // remove both models from the global list
00241         for (int i = (int) grasp_models.size() - 1; i >= 0; i--)
00242         {
00243           if (grasp_models[i].getID() == base.getID() || grasp_models[i].getID() == target.getID())
00244           {
00245             grasp_models.erase(grasp_models.begin() + i);
00246           }
00247         }
00248 
00249         // set a unique ID
00250         result.setID(id_counter++);
00251 
00252         // add a new edges with this new model
00253         for (size_t i = 0; i < grasp_models.size(); i++)
00254         {
00255           pair<uint32_t, uint32_t> edge(result.getID(), grasp_models[i].getID());
00256           edges.push_back(edge);
00257         }
00258 
00259         // add the new model
00260         grasp_models.push_back(result);
00261 
00262         // check if we are running debug
00263         if (debug_)
00264         {
00265           // generate the pose array
00266           geometry_msgs::PoseArray poses;
00267           for (size_t i = 0; i < result.getNumGrasps(); i++)
00268           {
00269             const graspdb::Pose &pose = result.getGrasp(i).getGraspPose();
00270             poses.header.frame_id = pose.getRobotFixedFrameID();
00271             poses.poses.push_back(pose.toROSPoseMessage());
00272           }
00273           // publish the poses and the resulting merged point cloud
00274           debug_poses_pub_.publish(poses);
00275           debug_pc_pub_.publish(*result.getPCLPointCloud());
00276         }
00277       }
00278     }
00279   }
00280 
00281   // remove any original (unmerged) models and save the rest
00282   feedback.message = "Saving new models...";
00283   as_.publishFeedback(feedback);
00284   for (int i = ((int) grasp_models.size()) - 1; i >= 0; i--)
00285   {
00286     if (grasp_models[i].isOriginal())
00287     {
00288       grasp_models.erase(grasp_models.begin() + i);
00289     } else
00290     {
00291       // attempt to store it
00292       graspdb::GraspModel new_model = grasp_models[i].toGraspModel();
00293       if (graspdb_->addGraspModel(new_model))
00294       {
00295         ROS_INFO("Added new model to the database with ID %d.", new_model.getID());
00296         new_model_ids.push_back(new_model.getID());
00297       }
00298       else
00299       {
00300         ROS_WARN("Error inserting model into database.");
00301       }
00302     }
00303   }
00304 }
00305 
00306 bool ModelGenerator::registrationCheck(const PCLGraspModel &base, const PCLGraspModel &target,
00307                                        PCLGraspModel &result) const
00308 {
00309   const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &base_pc = base.getPCLPointCloud();
00310   const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &target_pc = target.getPCLPointCloud();
00311   const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &result_pc = result.getPCLPointCloud();
00312 
00313   // perform ICP on the point clouds
00314   pcl::PointCloud<pcl::PointXYZRGB>::Ptr aligned_pc(new pcl::PointCloud<pcl::PointXYZRGB>);
00315   tf2::Transform tf_icp = point_cloud_metrics::performICP(base_pc, target_pc, aligned_pc);
00316 
00317   // check if the match is valid
00318   if (point_cloud_metrics::classifyMerge(base_pc, aligned_pc))
00319   {
00320     // transform each target grasp
00321     for (size_t i = 0; i < target.getNumGrasps(); i++)
00322     {
00323       // convert to tf2 matrix
00324       const graspdb::Grasp &old_grasp = target.getGrasp(i);
00325       const graspdb::Pose &old_grasp_pose = old_grasp.getGraspPose();
00326       tf2::Transform tf_pose = old_grasp_pose.toTF2Transform();
00327 
00328       // perform the transform
00329       tf2::Transform tf_result = tf_icp * tf_pose;
00330 
00331       // add the new grasp
00332       graspdb::Pose new_pose(old_grasp_pose.getRobotFixedFrameID(), tf_result);
00333       graspdb::Grasp new_grasp(new_pose, graspdb::GraspModel::UNSET_ID, old_grasp.getEefFrameID(),
00334                                old_grasp.getSuccesses(), old_grasp.getAttempts());
00335       result.addGrasp(new_grasp);
00336     }
00337 
00338     // add the base grasps
00339     for (size_t i = 0; i < base.getNumGrasps(); i++)
00340     {
00341       graspdb::Grasp grasp(base.getGrasp(i));
00342       // reset the ID
00343       grasp.setGraspModelID(graspdb::GraspModel::UNSET_ID);
00344       result.addGrasp(grasp);
00345     }
00346 
00347     // merge the two point clouds
00348     *result_pc = *base_pc + *aligned_pc;
00349 
00350     point_cloud_metrics::filterRedundantPoints(result_pc);
00351     // move to the origin
00352     point_cloud_metrics::transformToOrigin(result_pc, result.getGrasps());
00353 
00354     // set the final model parameters
00355     result.setObjectName(base.getObjectName());
00356     return true;
00357   } else
00358   {
00359     return false;
00360   }
00361 }


rail_recognition
Author(s): David Kent , Russell Toris , bhetherman
autogenerated on Thu Jun 6 2019 19:44:08