ClearpathTurtleRndmWalk.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 10Hz
00021 #define MAX_RUNTIME_SECONDS 0.1f
00022 
00023 // Typedef PCL Clouds
00024 typedef pcl::PointXYZRGB PointType;
00025 typedef pcl::PointCloud<PointType> PCLPointCloud;
00026 
00027 // Publishers
00028 ros::Publisher pubvel;
00029 
00030 // ROS Param Variables
00031 double lin_speed = 0.3;
00032 double ang_speed = 0.75;
00033 double still_ang_speed = 2.0;
00034 double cam_height = 0.55;
00035 double stop_distance = 0.8;
00036 int stop_point_count = 50;
00037 
00038 // Floor Normal
00039 bool normal_initialized = false;
00040 double base_normal[4] = {0.0};
00041 double last_normal[4] = {0.0};
00042 
00043 // PointClouds
00044 PointCloud cloudCopy;
00045 PointCloud cloudCut;
00046 PointCloud upright;
00047 PointCloud slice;
00048 
00049 // Timers
00050 double start_command = 0.0;
00051 double turn_time = 0.0;
00052 
00053 // Robot States
00054 double my_ang_speed = 0.0;
00055 bool stop_mode = false;
00056 
00057 // Main Callback
00058 void callback(const PCLPointCloud::ConstPtr& cloud)
00059 {
00060     // Clear all point clouds
00061     cloudCopy.points.clear();
00062     cloudCut.points.clear();
00063     upright.points.clear();
00064     slice.points.clear();
00065 
00066     // Start Timer
00067     ros::Time begin = ros::Time::now();
00068 
00069 
00070     // Convert point cloud to the Clearpath type (could optimize by removing this... unfortunately ClearpathDemoTools has to work without ROS and PCL)
00071     for (int i = 0; i < cloud->points.size(); i++)
00072     {
00073         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;
00074         cloudCopy.points.push_back(p);
00075     }
00076 
00077     // Initialize the floor plane one time.
00078     if (normal_initialized == false)
00079     {
00080         // Guess at Plane Points
00081         ClearpathDemoTools::VerticalCut(&cloudCopy, &cloudCut, base_normal[3]-0.3, base_normal[3]+0.3, 13);
00082 
00083         // Find the plane using RANSAC and Least Squares
00084         double norm[4];
00085         norm[0] = last_normal[0]; norm[1] = last_normal[1]; norm[2] = last_normal[2]; norm[3] = last_normal[3];
00086         if ( ClearpathDemoTools::PlaneRANSAC(&cloudCut, &norm[0], true) ) {
00087             ClearpathDemoTools::PlaneLS(&cloudCut, &norm[0]);
00088             last_normal[0] = norm[0]; last_normal[1] = norm[1]; last_normal[2] = norm[2]; last_normal[3] = norm[3];
00089             normal_initialized = true;
00090         } 
00091     }
00092 
00093     // Transform points such that the xz plane is where the calibrated floor was
00094     ClearpathDemoTools::TransformByNormal(&cloudCopy, &upright, &last_normal[0]);
00095 
00096     // Trim the cloud and only keep the region above the floor
00097     ClearpathDemoTools::VerticalCut(&upright, &slice, -1.0, -0.02, 1);
00098     
00099     // Find closest point
00100     unsigned closeCount = 0;
00101     for ( int i = 0; i < slice.points.size(); i++ )
00102     {
00103         double d = sqrt(slice.points[i].x*slice.points[i].x + slice.points[i].z*slice.points[i].z);
00104         if ( d < stop_distance )
00105             closeCount++;
00106     }
00107 
00108     // Choose Velocity
00109     geometry_msgs::Twist tw;
00110     tw.linear.x = 0.0;
00111     tw.angular.z = 0.0;
00112     
00113     if (stop_mode == true) // Was previously stopped and turning, continue unless breaks timeout
00114     {
00115         if (ros::Time::now().toSec() - start_command > turn_time) // Check if turn has timed out
00116         {
00117             stop_mode = false;
00118             my_ang_speed = 0.0;
00119         }
00120     } 
00121     else // Do motion as normal
00122     {
00123         // Generate random numbers
00124         srand ( time(NULL) );
00125         int rndm2 = rand() % 2; // 0 or 1
00126         int rndm3 = rand() % 3 - 1; // -1, 0 or 1
00127 
00128         if (closeCount > stop_point_count) // A cluster of points is within unsafe distance, stop!
00129         {
00130             stop_mode = true;
00131 
00132             // Choose a direction
00133             if (rndm2 == 1) my_ang_speed = still_ang_speed;
00134             else my_ang_speed = -still_ang_speed;
00135 
00136             // Start timer
00137             start_command = ros::Time::now().toSec();
00138 
00139             // Choose a random timeout
00140             turn_time = double(rand() % 3 + 1)*0.5;
00141         } 
00142         else // Safe to travel
00143         {
00144             // Has been travelling for 1 second safely, switch motion type
00145             if (ros::Time::now().toSec() - start_command > 1.0)
00146             {
00147                 my_ang_speed = rndm3*ang_speed;   
00148                 start_command = ros::Time::now().toSec();
00149             }
00150             
00151             // Set linear speed
00152             tw.linear.x = lin_speed;
00153         }
00154     }
00155 
00156     // Set angular speed
00157     tw.angular.z = my_ang_speed;
00158     pubvel.publish(tw);
00159 
00160     ros::Duration duration = ros::Time::now() - begin;
00161     if (duration.toSec() > MAX_RUNTIME_SECONDS) {
00162       ROS_WARN("Iteration of vision loop took %f seconds (max is %f)", duration.toSec(), MAX_RUNTIME_SECONDS);
00163     }
00164 }
00165 
00166 int main(int argc, char** argv)
00167 {
00168     ros::init (argc, argv, "turtle_default");    
00169 
00170     // Get ROS Params
00171     ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName());
00172     std::string in, out, out_vel;
00173     nh.param<std::string>("in", in, "rndmwalk_in");
00174     nh.param<std::string>("out_vel", out_vel, "rndmwalk_out_vel");
00175     nh.param<double>("lin_speed", lin_speed, 0.3);
00176     nh.param<double>("ang_speed", ang_speed, 0.75);
00177     nh.param<double>("still_ang_speed", still_ang_speed, 2.0);
00178     nh.param<double>("cam_height", cam_height, 0.3);
00179     nh.param<double>("stop_distance", stop_distance, 0.8);
00180     nh.param<int>("stop_point_count", stop_point_count, 50);
00181 
00182     // Init Timer
00183     start_command = ros::Time::now().toSec() - 5.0;
00184 
00185     // Subscribe to Kinect Cloud
00186     ros::NodeHandle nx;
00187     ros::Subscriber sub = nx.subscribe<PCLPointCloud>(in, 1, callback);
00188 
00189     // Set to FULL Turtlebot Mode
00190     ros::ServiceClient operation_mode_client;
00191     operation_mode_client = nx.serviceClient<turtlebot_node::SetTurtlebotMode>("/turtlebot_node/set_operation_mode");
00192     turtlebot_node::SetTurtlebotMode srvMsg;
00193     srvMsg.request.mode = 3;
00194     operation_mode_client.call(srvMsg);
00195 
00196     // Open up publisher channel
00197     pubvel = nx.advertise<geometry_msgs::Twist> (out_vel, 1);
00198 
00199     // Initialize Floor Plane Normal
00200     base_normal[0] = 0.0;
00201     base_normal[1] = 1.0;
00202     base_normal[2] = 0.0;
00203     base_normal[3] = cam_height;
00204     last_normal[0] = base_normal[0];
00205     last_normal[1] = base_normal[1];
00206     last_normal[2] = base_normal[2];
00207     last_normal[3] = base_normal[3];
00208 
00209     // Spin
00210     ros::spin();
00211 
00212     return (0);
00213 }
00214 
00215 
00216 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


turtlebot_rndmwalk
Author(s): Sean Anderson
autogenerated on Wed Jul 17 2013 11:01:07