grasp_localizer.cpp
Go to the documentation of this file.
00001 #include <agile_grasp/grasp_localizer.h>
00002 
00003 
00004 GraspLocalizer::GraspLocalizer(ros::NodeHandle& node, const std::string& cloud_topic, 
00005         const std::string& cloud_frame, int cloud_type, const std::string& svm_file_name, 
00006         const Parameters& params) 
00007   : cloud_left_(new PointCloud()), cloud_right_(new PointCloud()), 
00008   cloud_frame_(cloud_frame), svm_file_name_(svm_file_name), num_clouds_(params.num_clouds_), 
00009   num_clouds_received_(0), size_left_(0)
00010 {
00011   // subscribe to input point cloud ROS topic
00012   if (cloud_type == CLOUD_SIZED)
00013                 cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspLocalizer::cloud_sized_callback, this);
00014         else if (cloud_type == POINT_CLOUD_2)
00015                 cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspLocalizer::cloud_callback, this);
00016   
00017   // create ROS publisher for grasps
00018   grasps_pub_ = node.advertise<agile_grasp::Grasps>("grasps", 10);
00019   
00020   // create localization object and initialize its parameters
00021   localization_ = new Localization(params.num_threads_, true, params.plotting_mode_);
00022   localization_->setCameraTransforms(params.cam_tf_left_, params.cam_tf_right_);
00023   localization_->setWorkspace(params.workspace_);
00024   localization_->setNumSamples(params.num_samples_);
00025   localization_->setFingerWidth(params.finger_width_);
00026   localization_->setHandOuterDiameter(params.hand_outer_diameter_);
00027   localization_->setHandDepth(params.hand_depth_);
00028   localization_->setInitBite(params.init_bite_);
00029   localization_->setHandHeight(params.hand_height_);
00030                   
00031   min_inliers_ = params.min_inliers_;
00032   
00033         if (params.plotting_mode_ == Localization::RVIZ_PLOTTING)
00034         {
00035                 localization_->createVisualsPub(node, params.marker_lifetime_, cloud_frame_);
00036         }
00037 }
00038 
00039 
00040 void GraspLocalizer::cloud_callback(const sensor_msgs::PointCloud2ConstPtr& msg)
00041 {
00042   if (num_clouds_received_ == num_clouds_)
00043     return;
00044   
00045   // get point cloud from topic
00046   if (cloud_frame_.compare(msg->header.frame_id) != 0 
00047                         && cloud_frame_.compare("/" + msg->header.frame_id) != 0)
00048   {
00049     std::cout << "Input cloud frame " << msg->header.frame_id << " is not equal to parameter " << cloud_frame_ 
00050                         << std::endl;
00051     std::exit(EXIT_FAILURE);
00052   }
00053   
00054   if (num_clouds_received_ == 0)
00055     pcl::fromROSMsg(*msg, *cloud_left_);
00056   else if (num_clouds_received_ == 1)
00057     pcl::fromROSMsg(*msg, *cloud_right_);
00058   std::cout << "Received cloud # " << num_clouds_received_ << " with " << msg->height * msg->width << " points\n";
00059   num_clouds_received_++;
00060 }
00061 
00062 
00063 void GraspLocalizer::cloud_sized_callback(const agile_grasp::CloudSized& msg)
00064 {
00065   // get point cloud from topic
00066   if (cloud_frame_.compare(msg.cloud.header.frame_id) != 0)
00067   {
00068     std::cout << "Input cloud frame " << msg.cloud.header.frame_id << " is not equal to parameter "
00069       << cloud_frame_ << std::endl;
00070     std::exit(EXIT_FAILURE);
00071   }
00072   
00073   pcl::fromROSMsg(msg.cloud, *cloud_left_);
00074   size_left_ = msg.size_left.data;
00075   std::cout << "Received cloud with size_left: " << size_left_ << std::endl;
00076   num_clouds_received_ = 1;
00077 }
00078 
00079 
00080 void GraspLocalizer::localizeGrasps()
00081 {
00082   ros::Rate rate(1);
00083   std::vector<int> indices(0);
00084   
00085   while (ros::ok())
00086   {
00087     // wait for point clouds to arrive
00088     if (num_clouds_received_ == num_clouds_)
00089     {
00090       // localize grasps
00091       if (num_clouds_ > 1)
00092       {
00093         PointCloud::Ptr cloud(new PointCloud());
00094         *cloud = *cloud_left_ + *cloud_right_;
00095         hands_ = localization_->localizeHands(cloud, cloud_left_->size(), indices, false, false);
00096       }
00097       else
00098       {
00099         hands_ = localization_->localizeHands(cloud_left_, cloud_left_->size(), indices, false, false);
00100                         }
00101       
00102       antipodal_hands_ = localization_->predictAntipodalHands(hands_, svm_file_name_);
00103       handles_ = localization_->findHandles(antipodal_hands_, min_inliers_, 0.005);
00104       
00105       // publish handles
00106       grasps_pub_.publish(createGraspsMsg(handles_));
00107       ros::Duration(1.0).sleep();
00108       
00109       // publish hands contained in handles
00110       grasps_pub_.publish(createGraspsMsgFromHands(handles_));
00111       ros::Duration(1.0).sleep();
00112       
00113       // reset
00114       num_clouds_received_ = 0;
00115     }
00116     
00117     ros::spinOnce();
00118     rate.sleep();
00119   }
00120 }
00121 
00122 
00123 agile_grasp::Grasps GraspLocalizer::createGraspsMsg(const std::vector<GraspHypothesis>& hands)
00124 {
00125   agile_grasp::Grasps msg;
00126   
00127   for (int i = 0; i < hands.size(); i++)
00128         {
00129         msg.grasps.push_back(createGraspMsg(hands[i]));
00130   }
00131   
00132   msg.header.stamp = ros::Time::now();  
00133   return msg;
00134 }
00135 
00136 
00137 agile_grasp::Grasp GraspLocalizer::createGraspMsg(const GraspHypothesis& hand)
00138 {
00139   agile_grasp::Grasp msg;
00140   tf::vectorEigenToMsg(hand.getGraspBottom(), msg.center);
00141   tf::vectorEigenToMsg(hand.getAxis(), msg.axis);
00142   tf::vectorEigenToMsg(hand.getApproach(), msg.approach);
00143   tf::vectorEigenToMsg(hand.getGraspSurface(), msg.surface_center);
00144   msg.width.data = hand.getGraspWidth();
00145   return msg;
00146 }
00147 
00148 
00149 agile_grasp::Grasps GraspLocalizer::createGraspsMsgFromHands(const std::vector<Handle>& handles)
00150 {
00151   agile_grasp::Grasps msg;  
00152   for (int i = 0; i < handles.size(); i++)
00153   {
00154     const std::vector<GraspHypothesis>& hands = handles[i].getHandList();
00155     const std::vector<int>& inliers = handles[i].getInliers();
00156     
00157     for (int j = 0; j < inliers.size(); j++)
00158     {
00159       msg.grasps.push_back(createGraspMsg(hands[inliers[j]]));
00160     }
00161   }
00162   msg.header.stamp = ros::Time::now();
00163   std::cout << "Created grasps msg containing " << msg.grasps.size() << " hands\n";
00164   return msg;
00165 }
00166 
00167 
00168 agile_grasp::Grasps GraspLocalizer::createGraspsMsg(const std::vector<Handle>& handles)
00169 {
00170   agile_grasp::Grasps msg;  
00171   for (int i = 0; i < handles.size(); i++)
00172     msg.grasps.push_back(createGraspMsg(handles[i]));  
00173   msg.header.stamp = ros::Time::now();
00174   std::cout << "Created grasps msg containing " << msg.grasps.size() << " handles\n";
00175   return msg;
00176 }
00177 
00178 
00179 agile_grasp::Grasp GraspLocalizer::createGraspMsg(const Handle& handle)
00180 {
00181   agile_grasp::Grasp msg;
00182   tf::vectorEigenToMsg(handle.getCenter(), msg.center);
00183   tf::vectorEigenToMsg(handle.getAxis(), msg.axis);
00184   tf::vectorEigenToMsg(handle.getApproach(), msg.approach);
00185   tf::vectorEigenToMsg(handle.getHandsCenter(), msg.surface_center);
00186   msg.width.data = handle.getWidth();
00187   return msg;
00188 }


agile_grasp
Author(s): Andreas ten Pas
autogenerated on Sat Jun 8 2019 20:08:27