obstacles.cc
Go to the documentation of this file.
00001 #include <string>
00002 #include <ros/ros.h>
00003 #include <sensor_msgs/LaserScan.h>
00004 #include <pcl_ros/point_cloud.h>
00005 #include <pcl/point_types.h>
00006 
00007 namespace
00008 {
00009   ros::Subscriber subLaserScan_;
00010   ros::Publisher pubPointCloud_;
00011   pcl::PointCloud<pcl::PointXYZI> pc_;
00012 }
00013 
00014 void processLaserScan(const sensor_msgs::LaserScan &laserScan) 
00015 {
00016   // Pass the header information along (including frame ID)
00017   pc_.header = laserScan.header;
00018 
00019   int maxPoints = ceil((laserScan.angle_max - laserScan.angle_min)
00020                        / laserScan.angle_increment) + 1;
00021   pc_.points.resize(maxPoints);
00022 
00023   float currentAngle = laserScan.angle_min;
00024   int angleIndex = 0;
00025   int numPoints = 0;
00026   while(currentAngle <= laserScan.angle_max) {
00027 
00028     float distance = laserScan.ranges[angleIndex];
00029     
00030     // See if an obstacle was detected
00031     if (distance > laserScan.range_min && distance < laserScan.range_max) { 
00032       float xPt = cosf(currentAngle) * distance;
00033       float yPt = sinf(currentAngle) * distance;
00034 
00035       pc_.points[numPoints].x = xPt;
00036       pc_.points[numPoints].y = yPt;
00037       pc_.points[numPoints].z = 0.25;
00038       pc_.points[numPoints].intensity = laserScan.intensities[angleIndex];
00039       numPoints++;
00040     }
00041 
00042     angleIndex++;
00043     currentAngle += laserScan.angle_increment;
00044   }
00045 
00046   pc_.points.resize(numPoints);
00047 
00048   pubPointCloud_.publish(pc_);
00049 }
00050 
00051 int main(int argc, char *argv[]) 
00052 {
00053 
00054   ros::init(argc, argv, "art_simulated_obstacles");
00055   ros::NodeHandle node;
00056   
00057   // Subscribers
00058   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00059   subLaserScan_ =
00060       node.subscribe("front_sick", 10, &processLaserScan, noDelay);
00061   
00062   // Publishers
00063   pubPointCloud_ =
00064     node.advertise<pcl::PointCloud<pcl::PointXYZI> >("velodyne_obstacles", 10);
00065 
00066   ros::spin();                          // handle incoming data
00067  
00068   return 0;
00069 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


simulator_art
Author(s): Jack O'Quin
autogenerated on Tue Sep 24 2013 10:41:35