points_on_road.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2010 UT-Austin &  Austin Robot Technology, Michael Quinlan
00003  * 
00004  *  License: Modified BSD Software License 
00005  */
00006 
00019 #include <ros/ros.h>
00020 #include <sensor_msgs/PointCloud.h>
00021 
00022 #include <art_msgs/ArtLanes.h>
00023 
00024 #include <art_map/PolyOps.h>
00025 #include <tf/transform_listener.h>
00026 
00027 #include <string>
00028 
00029 #define NODE "maplanes_grid"
00030 
00031 static int qDepth = 1;                  // ROS topic queue size
00032 static ros::Publisher output;
00033 sensor_msgs::PointCloud pc;             // outgoing PointCloud message
00034 
00035 tf::TransformListener* listener;
00036 art_msgs::ArtLanes map;                 // local map
00037 PolyOps* pops;
00038 
00039 bool isPointInMap(float x, float y) 
00040 {
00041   int numPolys = map.polygons.size();
00042   for (int i=0; i<numPolys; i++)
00043     {
00044       poly p(map.polygons[i]);
00045       if (pops->pointInPoly(x,y,p))
00046         {
00047           return true;
00048         }
00049     }
00050   return false;
00051 }
00052 
00058 void processObstacles(const sensor_msgs::PointCloud &msg)
00059 {
00060    //ROS_INFO("num 3d points = %d", msg->points.size());  
00061         if(msg.points.size()<1)
00062                 return;
00063   
00064   sensor_msgs::PointCloud trans;
00065   try {
00066     listener->transformPointCloud("/map", msg, trans); 
00067 
00068   } catch (tf::TransformException ex){
00069     ROS_ERROR("%s",ex.what());
00070   }
00071  
00072   // pass along original time stamp and frame ID
00073   pc.header.stamp = trans.header.stamp;
00074   pc.header.frame_id = trans.header.frame_id;
00075 
00076   // set the exact point cloud size -- the vectors should already have
00077   // enough space
00078   size_t npoints = trans.points.size();
00079   pc.points.resize(npoints);
00080   size_t count=0;
00081   for (unsigned i = 0; i < npoints; ++i)
00082     {
00083       bool in = isPointInMap(trans.points[i].x,trans.points[i].y);
00084       if (in) {
00085         pc.points[count].x = trans.points[i].x;
00086         pc.points[count].y = trans.points[i].y;
00087         pc.points[count].z = trans.points[i].z;
00088         count++;
00089       }
00090     }
00091   pc.points.resize(count);
00092   output.publish(pc);
00093 }
00094 
00095 // \brief stores the values of the map
00096 void processMap(const art_msgs::ArtLanes &msg)
00097 {
00098   map = msg;
00099 }
00100 
00101 int main(int argc, char *argv[])
00102 {
00103   ros::init(argc, argv, NODE);
00104   ros::NodeHandle node;
00105 
00106   pops = new PolyOps();
00107   
00108   // subscribe to velodyne input -- make sure queue depth is minimal,
00109   // so any missed scans are discarded.  Otherwise latency gets out of
00110   // hand.  It's bad enough anyway.
00111   ros::Subscriber velodyne_obstacles =
00112     node.subscribe("velodyne/obstacles", qDepth, processObstacles,
00113                    ros::TransportHints().tcpNoDelay(true));
00114 
00115   
00116   ros::Subscriber road_map =
00117     node.subscribe("roadmap_local", qDepth, processMap,
00118                    ros::TransportHints().tcpNoDelay(true));
00119 
00120   output = node.advertise<sensor_msgs::PointCloud>("obstacles/points_on_road",
00121                                                    qDepth);
00122   
00123   listener = new tf::TransformListener();
00124  
00125   ros::spin();                          // handle incoming data
00126 
00127   return 0;
00128 }


art_observers
Author(s): Michael Quinlan, Jack O'Quin, Corbyn Salisbury
autogenerated on Fri Jan 3 2014 11:09:22