ClearpathDemoExplore.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 "ClearpathDemoTools.h"
00009 #include "geometry_msgs/Twist.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 lin_speed;
00024 double ang_speed;
00025 double cam_height;
00026 double cam_z_trans;
00027 double wall_buffer;
00028 double angular_vel_correction;
00029 int publish_visualization = 1;
00030 int random_walk = 0;
00031 
00032 // ROS-Independant Demo Tool
00033 ClearpathDemoTools demo;
00034 
00035 // PointCloud for drawing
00036 PCLPointCloud::Ptr final(new PCLPointCloud);
00037 
00038 // Callback from Kinect
00039 void callback(const PCLPointCloud::ConstPtr& cloud)
00040 {
00041     // Clear Result
00042     final->points.clear();
00043 
00044     // Start Timer
00045     ros::Time begin = ros::Time::now();
00046 
00047     // Convert PCL Point Cloud to our basic version
00048     // TODO This should be able to be done by a resize + memcpy
00049     PointCloud c, f;
00050     for (int i = 0; i < cloud->points.size(); i++)
00051     {
00052         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;
00053         c.points.push_back(p);
00054     }
00055 
00056     // Call Demo Update and get Twist
00057     Twist tw2 = demo.Update(&c, &f);
00058 
00059     // Convert Twist to ROS-Twist msg
00060     geometry_msgs::Twist tw;
00061     tw.linear.x = tw2.linear;
00062     tw.angular.z = angular_vel_correction*tw2.angular;
00063     pubvel.publish(tw);        
00064 
00065     if (publish_visualization == 1)
00066     {
00067         // Convert our basic PointCloud to PCL Type
00068         // TODO This should be able to be done by a resize + memcpy
00069         for (int i = 0; i < f.points.size(); i++)
00070         {
00071             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;
00072             final->points.push_back(p);
00073         }
00074         
00075         // Publish Visualization Cloud
00076         pub.publish(*final);
00077     }
00078 
00079     ros::Duration duration = ros::Time::now() - begin;
00080     if (duration.toSec() > MAX_RUNTIME_SECONDS) {
00081       ROS_WARN("Iteration of vision loop took %f seconds (max is %f)", duration.toSec(), MAX_RUNTIME_SECONDS);
00082     }
00083 }
00084 
00085 // Main
00086 int main(int argc, char** argv)
00087 {
00088     ros::init (argc, argv, "idfloor_default");    
00089 
00090     // Get ROS Params
00091     ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName());
00092     std::string in, out, out_vel;
00093     nh.param<std::string>("in", in, "idfloor_in");
00094     nh.param<std::string>("out", out, "idfloor_out");
00095     nh.param<std::string>("out_vel", out_vel, "idfloor_out_vel");
00096 
00097     nh.param<double>("angular_vel_correction", angular_vel_correction, 1.0);
00098     nh.param<double>("lin_speed", lin_speed, 0.3);
00099     nh.param<double>("ang_speed", ang_speed, 0.3);
00100     nh.param<double>("cam_height", cam_height, 0.55);
00101     nh.param<double>("cam_z_trans", cam_z_trans, 0.0);
00102     nh.param<double>("wall_buffer", wall_buffer, 0.6);
00103 
00104     nh.param<int>("publish_visualization", publish_visualization, 1);
00105     nh.param<int>("random_walk", random_walk, 0);
00106 
00107     // Init Demo Tool
00108     demo.InitExplore(lin_speed,
00109                      ang_speed,
00110                      cam_height,
00111                      cam_z_trans,
00112                      wall_buffer,
00113                      random_walk);
00114 
00115     // Init Subscribers
00116     ros::NodeHandle nx;
00117     ros::Subscriber sub = nx.subscribe<PCLPointCloud>(in, 1, callback);
00118 
00119     // Init Publishers
00120     pub = nx.advertise<PCLPointCloud> (out, 1);
00121     pubvel = nx.advertise<geometry_msgs::Twist> (out_vel, 1);
00122 
00123     // Set to Spin
00124     ros::spin();
00125 
00126     return (0);
00127 }
00128 
00129 
00130 


clearpath_kinect_demo
Author(s): Sean Anderson
autogenerated on Thu Jan 2 2014 11:06:26