ClearpathTurtleTrain.cpp
Go to the documentation of this file.
00001 // Includes
00002 
00003 #include <ros/ros.h>
00004 #include <pcl_ros/point_cloud.h>
00005 #include <pcl/point_types.h>
00006 #include "pcl/io/pcd_io.h"
00007 
00008 #include <algorithm>
00009 #include <cmath>
00010 #include <iostream>
00011 
00012 #include "Eigen/Dense"
00013 
00014 #include "Vector3.h"
00015 #include "ClearpathDemoTools.h"
00016 
00017 #include "geometry_msgs/Twist.h"
00018 #include "turtlebot_node/SetTurtlebotMode.h"
00019 
00020 // Slowest permissible is 5Hz
00021 #define MAX_RUNTIME_SECONDS 0.2f
00022 
00023 // Typedef PCL Clouds
00024 typedef pcl::PointXYZRGB PointType;
00025 typedef pcl::PointCloud<PointType> PCLPointCloud;
00026 
00027 // Turtle Structure
00028 struct Turtle
00029 {
00030     double x;
00031     double z;
00032     int count;
00033 };
00034 
00035 // Publishers
00036 ros::Publisher pub;
00037 ros::Publisher pubvel;
00038 
00039 // ROS Param Variables
00040 int debug_draw = 0;
00041 int num_ransac_iter = 500;
00042 double angular_vel_correction = 1.0;
00043 double lin_speed = 0.3;
00044 double lin_speed_var = 0.1;
00045 double ang_speed = 0.2;
00046 double cam_height = 0.55;
00047 double bot_window_size = 0.5;
00048 double bot_radius = 0.165;
00049 double ransac_thresh = 0.02;
00050 double ang_speed_gain = 1.0;
00051 double target_dist = 1.2;
00052 double min_stop_dist = 0.9;
00053 
00054 // Floor Normal
00055 bool normal_initialized = false;
00056 double base_normal[4] = {0.0};
00057 double last_normal[4] = {0.0};
00058 
00059 // Target Window
00060 double targetX = 0.0;
00061 double targetZ = 1.5;
00062 
00063 // PointClouds
00064 PointCloud cloudCopy;
00065 PointCloud cloudCut;
00066 PointCloud upright;
00067 PointCloud area;
00068 PointCloud slice;
00069 PointCloud inliers;
00070 PointCloud final;
00071 
00072 // Trim the cloud by only using points inside a certain a given circle (x, z, radius)
00073 void GetSearchArea(PointCloud* cloud, PointCloud* cloudout, double x, double z, double radius)
00074 {
00075     cloudout->points.clear();
00076     for (unsigned int i = 0; i < cloud->points.size(); i++)
00077     {   
00078         double mag = (cloud->points[i].x-x)*(cloud->points[i].x-x) + (cloud->points[i].z-z)*(cloud->points[i].z-z);
00079         if ( mag < radius*radius)
00080         {
00081             cloudout->points.push_back(cloud->points[i]);
00082         }
00083     }
00084 }
00085 
00086 // Randomly take 2 points in the cloud, project them on the xz plane and then return the two possible circle centers given a radius
00087 bool GetCircleFromRnd2(PointCloud* cloud, double* xzxz, double radius)
00088 {
00089     int i1, i2, i3, num;
00090     i1 = i2 = i3 = 0;
00091     num = cloud->points.size();
00092     
00093     if (num > 10) // less than 10 points is too few
00094     {
00095         Vector3 a1, a2;
00096 
00097         unsigned int counter = 0;
00098         while (i2 == i1 || (a1-a2).Length() > radius*2.0 || (a1-a2).Length() < 0.15) { // choose points that aren't too far away, not too close and not the exact same point
00099             counter++;
00100             if (counter > 5) // if we've done this 5 times without success, just quit, it probably doesn't exist or is hard to find
00101                 return false;
00102             i1 = rand() % num; 
00103             i2 = rand() % num; 
00104             a1.x = cloud->points[i1].x; a1.y = 0.0; a1.z = cloud->points[i1].z;
00105             a2.x = cloud->points[i2].x; a2.y = 0.0; a2.z = cloud->points[i2].z;
00106         }
00107 
00108         // Calculate the two circle centers given the two selected points
00109         Vector3 ax, ay, az;
00110         ax = a2 - a1; ax.Normalize();
00111         ay.x = 0.0; ay.y = 1.0; ay.z = 0.0;
00112         az = ax.Cross(ay); az.Normalize();
00113         double d1 = ((a1-a2).Length()/2.0);
00114         double d2 = sqrt(radius*radius - d1*d1);
00115         Vector3 p1, p2;
00116         p1 = a1 + ax*d1 + az*d2;
00117         p2 = a1 + ax*d1 - az*d2;
00118 
00119         // Fill return array
00120         xzxz[0] = p1.x;
00121         xzxz[1] = p1.z;
00122         xzxz[2] = p2.x;
00123         xzxz[3] = p2.z;
00124 
00125     } else {
00126         return false;
00127     }
00128 
00129     return true;
00130 }
00131 
00132 double distBetweenTurtles(Turtle t1, Turtle t2)
00133 {
00134     return sqrt((t1.x-t2.x)*(t1.x-t2.x) + (t1.z-t2.z)*(t1.z-t2.z));
00135 }
00136 
00137 // Finds all objects with a high enough quality to be considered a turtle
00138 bool FindTurtles(PointCloud* cloud, std::vector<Turtle>* turtles, double radius, double thresh)
00139 {
00140     turtles->clear();
00141 
00142     // Make it more random
00143     srand ( time(NULL) );
00144 
00145     int iter_count = 0;   
00146     double bestxz[2];
00147     while (iter_count < num_ransac_iter) // for x iterations:
00148     {   
00149         iter_count++;  
00150         
00151         double xzxz[4];
00152         
00153         // Get two circle centers from a random 2 points
00154         if (!GetCircleFromRnd2(cloud, &xzxz[0], radius))
00155             continue;
00156         
00157         // Try each circle
00158         for (unsigned int c = 0; c < 2; c++)
00159         {
00160             int count = 0;      
00161 
00162             Vector3 circ;
00163             circ.x = xzxz[c*2+0];
00164             circ.y = 0.0f;
00165             circ.z = xzxz[c*2+1];
00166 
00167             // Check to see how many points in the cloud lie on the surface of this cylinder (within a threshold)
00168             for (unsigned int i = 0; i < cloud->points.size(); i++)
00169             {   
00170                 Vector3 a1;
00171                 a1.x = cloud->points[i].x; a1.y = 0.0f; a1.z = cloud->points[i].z;
00172                 double mag = sqrt((a1.x-circ.x)*(a1.x-circ.x) + (a1.z-circ.z)*(a1.z-circ.z)) - radius;
00173                 if ( fabs(mag) < thresh) // Check if distance is less than threshold
00174                     count++;
00175             }
00176             
00177             // If the current consensus is better than a count of X
00178             if ( count > 75 )
00179             {
00180                 // Make a turtle
00181                 Turtle t; t.x = circ.x; t.z = circ.z; t.count = count;
00182                 bool replica = false;
00183 
00184                 // Check to see if we already have this turtle
00185                 for (int i = 0; i < turtles->size(); i++)
00186                 {
00187                     if (distBetweenTurtles((*turtles)[i], t) < radius/2.0)
00188                     {
00189                         replica = true;
00190                         if (t.count > (*turtles)[i].count)
00191                             (*turtles)[i] = t;
00192                     }
00193                 }
00194 
00195                 // Brand new turtle
00196                 if (replica == false)
00197                     turtles->push_back(t);
00198             }
00199         }
00200     }
00201 
00202     // Check that the best consensus has atleast x votes, otherwise it is probably garbage
00203     if (turtles->size() < 1)
00204         return false;
00205 
00206     return true;
00207 }
00208 
00209 // Main Callback
00210 void callback(const PCLPointCloud::ConstPtr& cloud)
00211 {
00212     bool found_target = false;
00213 
00214     // Clear all point clouds
00215     cloudCopy.points.clear();
00216     cloudCut.points.clear();
00217     upright.points.clear();
00218     area.points.clear();
00219     slice.points.clear();
00220     inliers.points.clear();
00221     final.points.clear();
00222 
00223     // Start Timer
00224     ros::Time begin = ros::Time::now();
00225 
00226 
00227     // Convert point cloud to the Clearpath type (could optimize by removing this... unfortunately ClearpathDemoTools has to work without ROS and PCL)
00228     for (int i = 0; i < cloud->points.size(); i++)
00229     {
00230         Point p;  p.x = cloud->points[i].x; p.y = cloud->points[i].y; p.z = cloud->points[i].z; p.rgb = cloud->points[i].rgb;
00231         cloudCopy.points.push_back(p);
00232     }
00233 
00234     // Initialize the floor plane one time.
00235     if (normal_initialized == false)
00236     {
00237         // Guess at Plane Points
00238         ClearpathDemoTools::VerticalCut(&cloudCopy, &cloudCut, base_normal[3]-0.3, base_normal[3]+0.3, 13);
00239 
00240         // Find the plane using RANSAC and Least Squares
00241         double norm[4];
00242         norm[0] = last_normal[0]; norm[1] = last_normal[1]; norm[2] = last_normal[2]; norm[3] = last_normal[3];
00243         if ( ClearpathDemoTools::PlaneRANSAC(&cloudCut, &norm[0], true) ) {
00244             ClearpathDemoTools::PlaneLS(&cloudCut, &norm[0]);
00245             last_normal[0] = norm[0]; last_normal[1] = norm[1]; last_normal[2] = norm[2]; last_normal[3] = norm[3];
00246             normal_initialized = true;
00247         } 
00248     }
00249 
00250     // Transform points such that the xz plane is where the calibrated floor was
00251     ClearpathDemoTools::TransformByNormal(&cloudCopy, &upright, &last_normal[0]);
00252 
00253     // Trim the cloud and only keep a small vertical portion (where the Create is)
00254     ClearpathDemoTools::VerticalCut(&upright, &slice, -0.08, -0.02, 1);
00255 
00256     // Trim the cloud again such that we only keep poings inside the search window
00257     GetSearchArea(&slice, &area, targetX, targetZ, bot_radius+bot_window_size);
00258     
00259     // Find all possible Turtles
00260     std::vector<Turtle> turtles;
00261     if ( FindTurtles(&slice, &turtles, bot_radius, ransac_thresh) )
00262     {
00263         // Search through turtles for the best turtle inside the window
00264         Turtle t; t.x = targetX; t.z = targetZ;
00265         int besti = 0;
00266         int bestcount = 0;
00267         for (int ti = 0; ti < turtles.size(); ti++)
00268         {
00269             if (distBetweenTurtles(t, turtles[ti]) < bot_window_size)
00270             {
00271                 if (turtles[ti].count > bestcount)
00272                 {
00273                     bestcount = turtles[ti].count;
00274                     besti = ti;
00275                     found_target = true;
00276                 }
00277             }
00278         }
00279 
00280         // Set the new target
00281         if (found_target == true)
00282         {
00283             targetX = turtles[besti].x;
00284             targetZ = turtles[besti].z;
00285         }
00286     }
00287 
00288     // Print out our new guess
00289     printf("Chasing the robot @ %fm in X and %fm in Z \n", targetX, targetZ);
00290     
00291     // We found a target
00292     if (found_target == true) 
00293     { 
00294         // We are following! calculate the radial path that intersects us with the targetXZ
00295         double distToTarget = sqrt(targetX*targetX + targetZ*targetZ);
00296         if (distToTarget > min_stop_dist) {
00297             Twist tw2 = ClearpathDemoTools::DetermineVelocity(targetX, targetZ, lin_speed);
00298             geometry_msgs::Twist tw;
00299             
00300             double err = distToTarget-target_dist; // positive means go faster
00301             tw.linear.x = tw2.linear + lin_speed_var*err/(target_dist-min_stop_dist);
00302 
00303             tw.angular.z = ang_speed_gain*tw2.angular; // although tw2.angular should put us right on the target, this usually needs a push..
00304             
00305             pubvel.publish(tw);
00306         }
00307         // We are within stop distance, turn to face the robot
00308         else { 
00309             geometry_msgs::Twist tw;
00310             tw.linear.x = 0.0;
00311             double tempspd = ang_speed;
00312             if (fabs(atan2(targetX,targetZ)) < 10.0/180.0*3.141592)
00313                 tempspd = 0.0;
00314             tw.angular.z = -ang_speed_gain*copysign(tempspd, targetX);
00315             pubvel.publish(tw);
00316         }
00317     } else {
00318         // Publish zero velocity
00319         geometry_msgs::Twist tw;
00320         tw.linear.x = 0.0;
00321         tw.angular.z = 0.0;
00322         pubvel.publish(tw);
00323     }
00324 
00325     if (debug_draw == 1)
00326     {
00327         PCLPointCloud f;
00328         for (int ti = 0; ti < turtles.size(); ti++)
00329         {
00330             PointCloud area2, red, final2;
00331             GetSearchArea(&upright, &area2, turtles[ti].x, turtles[ti].z, bot_radius+bot_window_size);
00332 
00333             // Get area around the guess for visualization
00334             ClearpathDemoTools::VerticalCut(&area2, &red, -0.02, 0.02, 1);
00335             ClearpathDemoTools::VerticalCut(&area2, &final2, -0.08, -0.02, 1);
00336 
00337             // Convert back to PCL Type for rendering
00338             for (int i = 0; i < final2.points.size(); i++)
00339             {
00340                 PointType p;  p.x = final2.points[i].x; p.y = final2.points[i].y; p.z = final2.points[i].z; p.rgb = final2.points[i].rgb;
00341                 double err = sqrt((p.x-turtles[ti].x)*(p.x-turtles[ti].x) + (p.z-turtles[ti].z)*(p.z-turtles[ti].z)) - bot_radius;
00342                 if ( fabs(err) < ransac_thresh) // Check if distance is less than threshold
00343                     f.points.push_back(p);
00344             }
00345 
00346             // Choose Color for Floor Points
00347             char c[4];
00348             c[0] = 0;             c[1] = 0;             c[2] = 0; 
00349             if ( sqrt((targetX-turtles[ti].x)*(targetX-turtles[ti].x) + (targetZ-turtles[ti].z)*(targetZ-turtles[ti].z)) < 0.0001)
00350                 c[1] = 255;
00351             else
00352                 c[2] = 255;
00353             float* cf = (float*)(&c[0]);
00354 
00355             // Add Floor Points to show circle
00356             for (int i = 0; i < red.points.size(); i++)
00357             {
00358                 PointType p;  p.x = red.points[i].x; p.y = red.points[i].y; p.z = red.points[i].z; p.rgb = *cf;
00359                 f.points.push_back(p);
00360             }
00361         }
00362 
00363         // Publish the visualization cloud
00364         pub.publish(f);
00365     }
00366 
00367     ros::Duration duration = ros::Time::now() - begin;
00368     if (duration.toSec() > MAX_RUNTIME_SECONDS) {
00369       ROS_WARN("Iteration of vision loop took %f seconds (max is %f)", duration.toSec(), MAX_RUNTIME_SECONDS);
00370     }
00371 }
00372 
00373 int main(int argc, char** argv)
00374 {
00375     ros::init (argc, argv, "turtle_default");    
00376 
00377     // Get ROS Params
00378     ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName());
00379     std::string in, out, out_vel;
00380     nh.param<std::string>("in", in, "idfloor_in");
00381     nh.param<std::string>("out", out, "idfloor_out");
00382     nh.param<std::string>("out_vel", out_vel, "idfloor_out_vel");
00383 
00384     nh.param<double>("lin_speed", lin_speed, 0.3);
00385     nh.param<double>("lin_speed_var", lin_speed_var, 0.1);
00386     nh.param<double>("ang_speed", ang_speed, 0.3);
00387     nh.param<double>("ang_speed_gain", ang_speed_gain, 1.0);
00388     nh.param<double>("target_dist", target_dist, 1.2);
00389     nh.param<double>("min_stop_dist", min_stop_dist, 0.9);
00390 
00391     nh.param<double>("cam_height", cam_height, 0.55);
00392     nh.param<double>("bot_radius", bot_radius, 0.165);
00393 
00394     nh.param<double>("ransac_thresh", ransac_thresh, 0.02);
00395     nh.param<int>("num_ransac_iter", num_ransac_iter, 500);
00396 
00397     double window_init_targetX, window_init_targetZ;
00398     nh.param<double>("window_size", bot_window_size, 0.5);
00399     nh.param<double>("window_init_targetX", window_init_targetX, 0.0);
00400     nh.param<double>("window_init_targetZ", window_init_targetZ, 1.5);
00401     targetX = window_init_targetX;
00402     targetZ = window_init_targetZ;
00403 
00404     nh.param<int>("debug_draw", debug_draw, 0);
00405 
00406     // Subscribe to Kinect Cloud
00407     ros::NodeHandle nx;
00408     ros::Subscriber sub = nx.subscribe<PCLPointCloud>(in, 1, callback);
00409 
00410     // Set to FULL Turtlebot Mode
00411     ros::ServiceClient operation_mode_client;
00412     operation_mode_client = nx.serviceClient<turtlebot_node::SetTurtlebotMode>("/turtlebot_node/set_operation_mode");
00413     turtlebot_node::SetTurtlebotMode srvMsg;
00414     srvMsg.request.mode = 3;
00415     operation_mode_client.call(srvMsg);
00416 
00417     // Open up publisher channels
00418     pub = nx.advertise<PCLPointCloud> (out, 1);
00419     pubvel = nx.advertise<geometry_msgs::Twist> (out_vel, 1);
00420 
00421     // Initialize Floor Plane Normal
00422     base_normal[0] = 0.0;
00423     base_normal[1] = 1.0;
00424     base_normal[2] = 0.0;
00425     base_normal[3] = cam_height;
00426     last_normal[0] = base_normal[0];
00427     last_normal[1] = base_normal[1];
00428     last_normal[2] = base_normal[2];
00429     last_normal[3] = base_normal[3];
00430 
00431     // Spin
00432     ros::spin();
00433 
00434     return (0);
00435 }
00436 
00437 
00438 


turtlebot_train
Author(s): Sean Anderson
autogenerated on Fri Jan 3 2014 11:15:37