ClearpathDemoTrack.cpp
Go to the documentation of this file.
00001 // Includes
00002 #include <algorithm>
00003 #include <iostream>
00004 #include <ros/ros.h>
00005 #include <pcl_ros/point_cloud.h>
00006 #include <pcl/point_types.h>
00007 #include "pcl/io/pcd_io.h"
00008 #include "geometry_msgs/Twist.h"
00009 #include "ClearpathDemoTools.h"
00010 
00011 // Slowest permissible is 10Hz
00012 #define MAX_RUNTIME_SECONDS 0.1f
00013 
00014 // Typedef PCL Types
00015 typedef pcl::PointXYZRGB PointType;
00016 typedef pcl::PointCloud<PointType> PCLPointCloud;
00017 
00018 // Publishers
00019 ros::Publisher pub;
00020 ros::Publisher pubvel;
00021 
00022 // Variables to get from ROS Param
00023 double angular_vel_correction = 1.0;
00024 double lin_speed = 0.2;
00025 double ang_speed = 0.2;
00026 double cam_height = 0.55;
00027 double cam_z_trans = 0.0;
00028 double window_size = 0.5;
00029 int publish_visualization = 1;
00030 
00031 // ROS-Independant Demo Tool
00032 ClearpathDemoTools demo;
00033 
00034 // PointCloud for drawing
00035 PCLPointCloud::Ptr final(new PCLPointCloud);
00036 
00037 // Callback from Kinect
00038 void callback(const PCLPointCloud::ConstPtr& cloud)
00039 {
00040     // Clear Result
00041     final->points.clear();
00042 
00043     // Start Timer
00044     ros::Time begin = ros::Time::now();
00045 
00046     // Convert PCL Point Cloud to our basic version
00047     // TODO This should be able to be done by a resize + memcpy
00048     PointCloud c, f;
00049     for (int i = 0; i < cloud->points.size(); i++)
00050     {
00051         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;
00052         c.points.push_back(p);
00053     }
00054 
00055     // Call Demo Update and get Twist
00056     Twist tw2 = demo.Update(&c, &f);
00057 
00058     // Convert Twist to ROS-Twist msg
00059     geometry_msgs::Twist tw;
00060     tw.linear.x = tw2.linear;
00061     tw.angular.z = angular_vel_correction*tw2.angular;
00062     pubvel.publish(tw);        
00063 
00064     if (publish_visualization == 1)
00065     {
00066         // Convert our basic PointCloud to PCL Type
00067         // TODO This should be able to be done by a resize + memcpy
00068         for (int i = 0; i < f.points.size(); i++)
00069         {
00070             PointType p;  p.x = f.points[i].x; p.y = f.points[i].y; p.z = f.points[i].z; p.rgb = f.points[i].rgb;
00071             final->points.push_back(p);
00072         }
00073         
00074         // Publish Visualization Cloud
00075         pub.publish(*final);
00076     }
00077 
00078     ros::Duration duration = ros::Time::now() - begin;
00079     if (duration.toSec() > MAX_RUNTIME_SECONDS) {
00080       ROS_WARN("Iteration of vision loop took %f seconds (max is %f)", duration.toSec(), MAX_RUNTIME_SECONDS);
00081     }
00082 }
00083 
00084 // Main
00085 int main(int argc, char** argv)
00086 {
00087     ros::init (argc, argv, "idfloor_default");    
00088 
00089     // Get ROS Params
00090     ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName());
00091     std::string in, out, out_vel;
00092     nh.param<std::string>("in", in, "idfloor_in");
00093     nh.param<std::string>("out", out, "idfloor_out");
00094     nh.param<std::string>("out_vel", out_vel, "idfloor_out_vel");
00095 
00096     nh.param<double>("angular_vel_correction", angular_vel_correction, 1.0);
00097     nh.param<double>("lin_speed", lin_speed, 0.3);
00098     nh.param<double>("ang_speed", ang_speed, 0.3);
00099     nh.param<double>("cam_height", cam_height, 0.55);
00100     nh.param<double>("cam_z_trans", cam_z_trans, 0.0);
00101     nh.param<double>("window_size", window_size, 0.5);
00102 
00103     nh.param<int>("publish_visualization", publish_visualization, 1);
00104 
00105     // Init Demo Tool
00106     demo.InitTrack(lin_speed,
00107                    ang_speed,
00108                    cam_height,
00109                    cam_z_trans,
00110                    window_size);
00111 
00112     // Init Subscribers
00113     ros::NodeHandle nx;
00114     ros::Subscriber sub = nx.subscribe<PCLPointCloud>(in, 1, callback);
00115 
00116     // Init Publishers
00117     pub = nx.advertise<PCLPointCloud> (out, 1);
00118     pubvel = nx.advertise<geometry_msgs::Twist> (out_vel, 1);
00119 
00120     // Set to Spin
00121     ros::spin();
00122 
00123     return (0);
00124 }
00125 
00126 
00127 


clearpath_kinect_demo
Author(s): Sean Anderson
autogenerated on Sun Oct 5 2014 22:52:46