lane_observations.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2010 UT-Austin & Austin Robot Technology, Michael Quinlan
00003  *  Copyright (C) 2011 UT-Austin & Austin Robot Technology
00004  *  License: Modified BSD Software License
00005  *
00006  *  $Id$
00007  */
00008 
00021 #include <sensor_msgs/point_cloud_conversion.h>
00022 #include <art_observers/lane_observations.h>
00023 #include <art_observers/QuadrilateralOps.h>
00024 
00026 LaneObservations::LaneObservations(ros::NodeHandle &node,
00027                                    ros::NodeHandle &priv_nh):
00028   node_(node),
00029   priv_nh_(priv_nh),
00030   config_(priv_nh),
00031   nearest_forward_observer_(config_),
00032   nearest_backward_observer_(config_),
00033   adjacent_left_observer_(config_),
00034   adjacent_right_observer_(config_),
00035   tf_listener_(new tf::TransformListener())
00036 { 
00037   // subscribe to point cloud topics
00038   pc_sub_ =
00039     node_.subscribe("velodyne/obstacles", 1,
00040                     &LaneObservations::processPointCloud, this,
00041                     ros::TransportHints().tcpNoDelay(true));
00042   pc2_sub_ =
00043     node_.subscribe("obstacles", 1,
00044                     &LaneObservations::processPointCloud2, this,
00045                     ros::TransportHints().tcpNoDelay(true));
00046 
00047   // subscribe to local road map
00048   road_map_sub_ =
00049     node_.subscribe("roadmap_local", 1,
00050                     &LaneObservations::processLocalMap, this,
00051                     ros::TransportHints().tcpNoDelay(true));
00052 
00053   // subscribe to odometry
00054   odom_sub_ = 
00055     node_.subscribe("odom", 1,
00056                     &LaneObservations::processPose, this,
00057                     ros::TransportHints().tcpNoDelay(true));
00058 
00059   // advertise published topics
00060   const std::string viz_topic("visualization_marker_array");
00061   viz_pub_ =
00062     node_.advertise<visualization_msgs::MarkerArray>(viz_topic, 1, true);
00063   observations_pub_ =
00064     node_.advertise <art_msgs::ObservationArray>("observations", 1, true);
00065 
00066   // Initialize observers.  They will be updated in this order.
00067   addObserver(nearest_forward_observer_);
00068   addObserver(nearest_backward_observer_);
00069   addObserver(adjacent_left_observer_);
00070   addObserver(adjacent_right_observer_);
00071 }
00072 
00074 LaneObservations::~LaneObservations() {}
00075 
00080 void LaneObservations::processObstacles(void) 
00081 {
00082   observations_.header.stamp = obstacles_.header.stamp;
00083   obs_quads_.polygons.clear();
00084   transformPointCloud(obstacles_);
00085   
00086   // skip the rest until the local road map has been received at least once
00087   if (local_map_.header.stamp > ros::Time())
00088     {
00089       filterPointsInLocalMap();
00090       runObservers();
00091       publishObstacleVisualization();
00092     }
00093 }
00094 
00096 void LaneObservations::processPointCloud(const sensor_msgs::PointCloud::ConstPtr &msg)
00097 {
00098   // convert to PointCloud2, then process that
00099   boost::shared_ptr<sensor_msgs::PointCloud2> pc2(new sensor_msgs::PointCloud2);
00100   sensor_msgs::convertPointCloudToPointCloud2(*msg, *pc2);
00101   processPointCloud2(pc2);
00102 }
00103 
00105 void LaneObservations::processPointCloud2(const sensor_msgs::PointCloud2::ConstPtr &msg)
00106 {
00107   // convert ROS message into PCL point cloud
00108   pcl::fromROSMsg(*msg, obstacles_);
00109 
00110   // process the PCL point cloud data
00111   processObstacles();
00112 }
00113 
00115 void LaneObservations::processLocalMap(const art_msgs::ArtLanes::ConstPtr &msg) 
00116 {
00117   local_map_ = *msg;
00118 }
00119 
00121 void LaneObservations::processPose(const nav_msgs::Odometry &odom)
00122 {
00123   pose_ = MapPose(odom.pose.pose);
00124 }
00125 
00127 void LaneObservations::filterPointsInLocalMap() 
00128 {
00129   // set the exact point cloud size
00130   size_t npoints = obstacles_.points.size();
00131   added_quads_.clear();
00132   for (unsigned i = 0; i < npoints; ++i)
00133     {
00134       isPointInAPolygon(obstacles_.points[i].x,
00135                         obstacles_.points[i].y);
00136     }
00137 }
00138 
00140 void LaneObservations::transformPointCloud(const PtCloud &msg) 
00141 {
00142   try
00143     {
00144       // wait for transform to be available
00145       tf_listener_->waitForTransform(config_.map_frame_id, msg.header.frame_id,
00146                                      msg.header.stamp, ros::Duration(0.2));
00147       pcl_ros::transformPointCloud(config_.map_frame_id, msg, obstacles_,
00148                                    *tf_listener_);
00149       observations_.header.frame_id = obstacles_.header.frame_id;
00150 
00151       // hopefully not needed in future
00152       calcRobotPolygon();
00153     }
00154   catch (tf::TransformException ex)
00155     {
00156       // only log tf error once every 20 times
00157       ROS_WARN_STREAM_THROTTLE(20, ex.what());
00158     }
00159 }
00160 
00168 bool LaneObservations::isPointInAPolygon(float x, float y) 
00169 {
00170   size_t num_polys = local_map_.polygons.size();
00171   
00172   bool inside = false;
00173   std::pair<std::tr1::unordered_set<int>::iterator, bool> pib;
00174   
00175   for (size_t i=0; i<num_polys; i++)
00176     {
00177       art_msgs::ArtQuadrilateral *p= &(local_map_.polygons[i]);
00178       float dist= ((p->midpoint.x-x)*(p->midpoint.x-x)
00179                    + (p->midpoint.y-y)*(p->midpoint.y-y));
00180 
00181       if (dist > 16)         // quick check: are we near the polygon?
00182         continue;
00183 
00184       inside = quad_ops::quickPointInPolyRatio(x,y,*p,0.6);
00185       // Add polygon to lane if so
00186       if (inside)
00187         {
00188           pib = added_quads_.insert(p->poly_id);
00189           if (pib.second)
00190             {
00191               obs_quads_.polygons.push_back(*p);
00192             }
00193         }
00194     }
00195 
00196   return inside;
00197 }
00198 
00200 void LaneObservations::runObservers() 
00201 {
00202   // update all the registered observers
00203   for (unsigned i = 0; i < observers_.size(); ++i)
00204     {
00205       observations_.obs[i] =
00206         observers_[i]->update(local_map_, obs_quads_, pose_);
00207     }
00208 
00209   // Publish their observations
00210   observations_pub_.publish(observations_);
00211 }                                                            
00212 
00214 void LaneObservations::calcRobotPolygon() 
00215 {
00216   // --- Hack to get my own polygon
00217   geometry_msgs::PointStamped laser_point;
00218   laser_point.header.frame_id = config_.robot_frame_id;  
00219   laser_point.header.stamp = ros::Time();
00220   
00221   laser_point.point.x = 0.0;
00222   laser_point.point.y = 0.0;
00223   laser_point.point.z = 0.0;
00224   
00225   geometry_msgs::PointStamped robot_point;
00226   tf_listener_->transformPoint(config_.map_frame_id, laser_point, robot_point);
00227 
00228   size_t numPolys = local_map_.polygons.size();
00229   float x = robot_point.point.x; 
00230   float y = robot_point.point.y;
00231   bool inside=false;
00232   for (size_t i=0; i<numPolys; i++)
00233     {
00234       art_msgs::ArtQuadrilateral *p= &(local_map_.polygons[i]);
00235       float dist = ((p->midpoint.x-x)*(p->midpoint.x-x)
00236                     + (p->midpoint.y-y)*(p->midpoint.y-y));
00237 
00238       if (dist > 16)          // quick check: we are near the polygon?
00239         continue;
00240 
00241       inside = quad_ops::quickPointInPoly(x,y,*p); 
00242       if (inside)
00243         {
00244           robot_polygon_ = *p;
00245         }
00246     }
00247 }
00248 
00250 void LaneObservations::publishObstacleVisualization()
00251 {
00252   if (viz_pub_.getNumSubscribers()==0)
00253     return;
00254 
00255   ros::Time now = ros::Time::now();
00256 
00257   // clear message array, this is a class variable to avoid memory
00258   // allocation and deallocation on every cycle
00259   marks_msg_.markers.clear();
00260 
00261   int i=0;
00262   for (obs_it_=obs_quads_.polygons.begin();
00263        obs_it_!=obs_quads_.polygons.end(); obs_it_++)
00264     {
00265       visualization_msgs::Marker mark;
00266       mark.header.stamp = now;
00267       mark.header.frame_id = config_.map_frame_id;
00268     
00269       mark.ns = "obstacle_polygons";
00270       mark.id = (int32_t) i;
00271       mark.type = visualization_msgs::Marker::CUBE;
00272       mark.action = visualization_msgs::Marker::ADD;
00273     
00274       mark.pose.position = obs_it_->midpoint;
00275       mark.pose.orientation =
00276         tf::createQuaternionMsgFromYaw(obs_it_->heading);
00277     
00278       mark.scale.x = 1.5;
00279       mark.scale.y = 1.5;
00280       mark.scale.z = 0.1;
00281       mark.lifetime =  ros::Duration(0.2);
00282     
00283       mark.color.a = 0.8;       // way-points are slightly transparent
00284       mark.color.r = 0.0;
00285       mark.color.g = 0.0;
00286       mark.color.b = 1.0;
00287     
00288       marks_msg_.markers.push_back(mark);
00289       i++;
00290     }
00291 
00292   // Draw the polygon containing the robot
00293   visualization_msgs::Marker mark;
00294   mark.header.stamp = now;
00295   mark.header.frame_id = config_.map_frame_id;
00296   
00297   mark.ns = "obstacle_polygons";
00298   mark.id = (int32_t) i;
00299   mark.type = visualization_msgs::Marker::CUBE;
00300   mark.action = visualization_msgs::Marker::ADD;
00301     
00302   mark.pose.position = robot_polygon_.midpoint;
00303   mark.pose.orientation =
00304     tf::createQuaternionMsgFromYaw(robot_polygon_.heading);
00305     
00306   mark.scale.x = 1.5;
00307   mark.scale.y = 1.5;
00308   mark.scale.z = 0.1;
00309   mark.lifetime =  ros::Duration(0.2);
00310   
00311   mark.color.a = 0.8;           // way-points are slightly transparent
00312   mark.color.r = 0.3;
00313   mark.color.g = 0.7;
00314   mark.color.b = 0.9;
00315   
00316   marks_msg_.markers.push_back(mark);
00317 
00318   // Publish the markers
00319   viz_pub_.publish(marks_msg_);
00320 }
00321 
00323 void LaneObservations::spin()
00324 {
00325   ros::spin();
00326 }


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