grasp_pcd.cpp
Go to the documentation of this file.
00001 
00002 #include <ros/ros.h>
00003 #include <sensor_msgs/PointCloud.h>
00004 #include <SimpleGraspPlanner.h>
00005 #include <geometry_msgs/Pose.h>
00006 
00007 
00008 using namespace sensor_msgs;
00009 
00010 bool s_need_cloud_data = true;
00011 ros::Publisher pub;
00012 std::string s_input_cloud_topic = "graspable_segments";
00013 std::string s_output_cloud_topic = "hand_config";
00014 
00025 double param_start_top = 0.0;
00026 double param_start_side = 0.0;
00030 void cloud_cb (const PointCloudConstPtr& cloud)
00031 {
00032   /*
00033   Cite sgp:
00034 
00035   typedef struct
00036   {
00037      double sx; double sxy; double sxz;
00038      double syx; double sy; double syz;
00039       double szx; double szy; double sz;
00040   }
00041   CovariancePoint;
00042   */
00043   CovariancePoint cov;   
00044   Point3D mean, a, b, c1,c2,c3;
00045   mean.x = 0.0;
00046   mean.y = 0.0;
00047   mean.z = 0.0;
00048   for(size_t i = 0; i < (*cloud).points.size(); i++)
00049   {
00050     mean.x += (*cloud).points[i].x;
00051     mean.y += (*cloud).points[i].y;
00052     mean.z += (*cloud).points[i].z;
00053   }
00054   mean.x /=  (*cloud).points.size();
00055   mean.y /=  (*cloud).points.size();
00056   mean.z /=  (*cloud).points.size();
00057   cov.sx  = 0.0;
00058   cov.sxy = 0.0;
00059   cov.sxz = 0.0;
00060   cov.syx = 0.0;
00061   cov.sy  = 0.0;
00062   cov.syz = 0.0;
00063   cov.szx = 0.0;
00064   cov.szy = 0.0;
00065   cov.sz  = 0.0;
00066   for(size_t i = 0; i < (*cloud).points.size(); i++)
00067   {
00068      double tmpx = (*cloud).points[i].x - mean.x;
00069      double tmpy = (*cloud).points[i].y - mean.y;
00070      double tmpz = (*cloud).points[i].z - mean.z;
00071 
00072      cov.sx  += tmpx*tmpx;
00073      cov.sxy += tmpx*tmpy;
00074      cov.sxz += tmpx*tmpz;
00075      cov.syx += tmpy*tmpx;
00076      cov.sy  += tmpy*tmpy;
00077      cov.syz += tmpy*tmpz;
00078      cov.szx += tmpz*tmpx;
00079      cov.szy += tmpz*tmpy;
00080      cov.sz  += tmpz*tmpz;
00081   }
00082   cov.sx  /= (*cloud).points.size();
00083   cov.sxy /= (*cloud).points.size();
00084   cov.sxz /= (*cloud).points.size();
00085   cov.syx /= (*cloud).points.size();
00086   cov.sy  /= (*cloud).points.size();
00087   cov.syz /= (*cloud).points.size();
00088   cov.szx /= (*cloud).points.size();
00089   cov.szy /= (*cloud).points.size();
00090   cov.sz  /= (*cloud).points.size();
00091   HandConfig cfg = GetGraspLM(&mean, &cov,1, param_start_top, param_start_side);
00092   geometry_msgs::Point32 pout;
00093   pout.x= cfg.alpha;
00094   pout.y = cfg.beta;
00095   pout.z  = cfg.delta_max;
00101   b.x = 0.0;
00102   b.y = 0.0;
00103   b.z = 0.0;
00104   a.x = 1.0;
00105   a.y = 0.0;
00106   a.z = 0.0;
00107   c1 = TransformPoint(a, b, cfg.alpha, cfg.beta, cfg.delta_max) ;
00108   a.x = 0.0;
00109   a.y = 1.0;
00110   a.z = 0.0;
00111   c2 = TransformPoint(a, b, cfg.alpha, cfg.beta, cfg.delta_max) ;
00112   a.x = 0.0;
00113   a.y = 0.0;
00114   a.z = 1.0;
00115   c3 = TransformPoint(a, b, cfg.alpha, cfg.beta, cfg.delta_max) ;
00116   geometry_msgs::Pose pose;
00117   pose.position.x = 0.0;
00118   pose.position.y = 0.0;
00119   pose.position.z = 0.0;
00120 
00123   double S, T = 1 + c1.x + c2.y + c3.z;
00124 
00125 
00126   /* If the trace of the matrix is greater than zero, then
00127   perform an "instant" calculation.
00128   Important note wrt. rouning errors: */
00129   if ( T > 0.00000001 )
00130   {
00131       S = sqrt(T) * 2;
00132       pose.orientation.x = ( c2.z - c3.y ) / S;
00133       pose.orientation.y = ( c3.x - c1.z ) / S;
00134       pose.orientation.z = ( c1.y - c2.x ) / S;
00135       pose.orientation.w = 0.25 * S;
00136   }
00137   else
00138   {
00139 
00140     /*If the trace of the matrix is equal to zero then identify
00141     which major diagonal element has the greatest value.
00142     Deending on this, calculate the following:*/
00143 
00144     if ( c1.x > c2.y && c1.x > c3.z )  {        // Column 0:
00145         S  = sqrt( 1.0 + c1.x - c2.y - c3.z ) * 2;
00146         pose.orientation.x = 0.25 * S;
00147         pose.orientation.y = (c1.y + c2.x ) / S;
00148         pose.orientation.z = (c3.x + c1.z ) / S;
00149         pose.orientation.w = (c2.z - c3.y ) / S;
00150     } else if ( c2.y > c3.z ) {                 // Column 1:
00151         S  = sqrt( 1.0 + c2.y - c1.x - c3.z ) * 2;
00152         pose.orientation.x = (c1.y + c2.x ) / S;
00153         pose.orientation.y = 0.25 * S;
00154         pose.orientation.z = (c2.z + c3.y ) / S;
00155         pose.orientation.w = (c3.x - c1.z ) / S;
00156     } else {                                            // Column 2:
00157         S  = sqrt( 1.0 + c3.z - c1.x - c2.y ) * 2;
00158         pose.orientation.x = (c3.x + c1.z ) / S;
00159         pose.orientation.y = (c2.z + c3.y ) / S;
00160         pose.orientation.z = 0.25 * S;
00161         pose.orientation.w = (c1.y - c2.x ) / S;
00162     }
00163   }
00164 
00165   pub.publish(pose);
00166 }
00167 
00168 #ifndef MAX_HAND_POINTS
00169 #define MAX_HAND_POINTS 100
00170 #endif
00171 Point3D hand_points[MAX_HAND_POINTS];
00172 
00173 void GetParams(ros::NodeHandle &n)
00174 {
00175   using namespace XmlRpc;
00176   int counting_hand_points = 0;
00177 
00178   if( n.hasParam( "sgp_config" ) )
00179   {
00180       XmlRpcValue sgp_config_param_value;
00181       n.getParam( "sgp_config", sgp_config_param_value );
00182       if( sgp_config_param_value.getType() != XmlRpcValue::TypeArray )
00183       {
00184           printf("Type %d\n", sgp_config_param_value.getType());
00185           ROS_WARN("sgp_config has invalid type.");
00186       }
00187       else
00188       {
00189         for(int i=0; i<sgp_config_param_value.size(); i++)
00190         {
00191             if( sgp_config_param_value[i].getType() != XmlRpcValue::TypeArray && sgp_config_param_value[i].size() != 3)
00192             {
00193                ROS_WARN("Ignoring sgp_config entry: Not a point consisting of three real values");
00194                continue;
00195             }
00196 
00197             if( sgp_config_param_value[i][0].getType() !=  XmlRpcValue::TypeDouble ||
00198                 sgp_config_param_value[i][1].getType() !=  XmlRpcValue::TypeDouble ||
00199                 sgp_config_param_value[i][2].getType() !=  XmlRpcValue::TypeDouble)
00200             {
00201               ROS_WARN("Ignoring sgp_config entry: Not a point consisting of three real values");
00202               continue;
00203             }
00204             hand_points[counting_hand_points].x = sgp_config_param_value[i][0];
00205             hand_points[counting_hand_points].y = sgp_config_param_value[i][1];
00206             hand_points[counting_hand_points].z = sgp_config_param_value[i][2];
00207             printf("Added point %d with %f %f %f\n", counting_hand_points, hand_points[counting_hand_points].x,
00208                         hand_points[counting_hand_points].y, hand_points[counting_hand_points].z);
00209             counting_hand_points++;
00210         }
00211       }
00212   }
00213   else
00214     printf("sgp config not set!!\n\n");
00215   if(n.hasParam( "sgp_config_param_start_top"))
00216   {
00217     XmlRpcValue sgp_config_param_value;
00218     n.getParam( "sgp_config_param_start_top", sgp_config_param_value );
00219 
00220     if(sgp_config_param_value.getType() !=  XmlRpcValue::TypeDouble)
00221        ROS_WARN("Ignoring sgp_config_paramt_start_top: Not a real value");
00222     else
00223     {
00224       param_start_top = sgp_config_param_value;
00225     }
00226   }
00227   if(n.hasParam( "sgp_config_param_start_side"))
00228   {
00229     XmlRpcValue sgp_config_param_value;
00230     n.getParam( "sgp_config_param_start_side", sgp_config_param_value );
00231     if(sgp_config_param_value.getType() !=  XmlRpcValue::TypeDouble)
00232        ROS_WARN("Ignoring sgp_config_param_start_side: Not a real value");
00233     else
00234     {
00235       param_start_side = sgp_config_param_value;
00236     }
00237   }
00238   InitSGP(hand_points, counting_hand_points, 0.0);
00239 }
00240 
00241 int main(int argc, char* argv[])
00242 {
00243   ros::init (argc, argv, "trans_pcd_jlo");
00244   ros::NodeHandle nh("~");
00245 
00246   ros::Subscriber cloud_sub = nh.subscribe (s_input_cloud_topic, 1, &cloud_cb);
00247   pub = nh.advertise<geometry_msgs::Pose>(s_output_cloud_topic, 1);
00248   GetParams(nh);
00249   while(nh.ok())
00250   {
00251     ros::spinOnce ();
00252   }
00253   return 0;
00254 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pc_grasp_position
Author(s): Ulrich F Klank
autogenerated on Thu May 23 2013 09:23:35