lane_observations.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*-
00002  *
00003  *  Copyright (C) 2011 Austin Robot Technology
00004  *  License: Modified BSD Software License Agreement
00005  * 
00006  *  $Id: lane_observations.h 1854 2011-11-17 04:49:55Z cor0505@aol.com $
00007  */
00008 
00018 #ifndef _LANE_OBSERVATIONS_H_
00019 #define _LANE_OBSERVATIONS_H_
00020 
00021 #include <vector>
00022 #include <tr1/unordered_set>
00023 
00024 #include <ros/ros.h>
00025 
00026 #include <art_msgs/ArtLanes.h>
00027 #include <art_msgs/ObservationArray.h>
00028 #include <pcl/io/pcd_io.h>
00029 #include <pcl/point_cloud.h>
00030 #include <pcl/point_types.h>
00031 #include <pcl_ros/transforms.h>
00032 #include <sensor_msgs/PointCloud.h>
00033 #include <sensor_msgs/PointCloud2.h>
00034 #include <tf/transform_listener.h>
00035 #include <visualization_msgs/MarkerArray.h>
00036 #include <nav_msgs/Odometry.h>
00037 
00038 #include <art_observers/nearest_forward.h>
00039 #include <art_observers/nearest_backward.h>
00040 #include <art_observers/adjacent_left.h>
00041 #include <art_observers/adjacent_right.h>
00042 
00043 #include <art_observers/ObserversConfig.h>
00044 typedef art_observers::ObserversConfig Config;
00045 
00046 class LaneObservations 
00047 {
00048 public:
00049 
00050   LaneObservations(ros::NodeHandle &node,
00051                    ros::NodeHandle &priv_nh);
00052   ~LaneObservations();
00053   void spin();
00054 
00055 private:
00056 
00058   typedef pcl::PointCloud<pcl::PointXYZ> PtCloud;
00059 
00064   void addObserver(observers::Observer &obs)
00065   {
00066     observers_.push_back(&obs);
00067     observations_.obs.push_back(art_msgs::Observation());
00068   }
00069 
00070   void calcRobotPolygon();
00071   void filterPointsInLocalMap();
00072   bool isPointInAPolygon(float x, float y);
00073   void processLocalMap(const art_msgs::ArtLanes::ConstPtr &msg);
00074   void processObstacles(void);
00075   void processPointCloud(const sensor_msgs::PointCloud::ConstPtr &msg);
00076   void processPointCloud2(const sensor_msgs::PointCloud2::ConstPtr &msg);
00077   void processPose(const nav_msgs::Odometry &odom);
00078   void publishObstacleVisualization();
00079   void runObservers();
00080   void transformPointCloud(const PtCloud &msg);
00081 
00082   ros::NodeHandle node_;                
00083   ros::NodeHandle priv_nh_;             
00084 
00085   Config config_;                       
00086 
00087   // observer instances
00088   observers::NearestForward nearest_forward_observer_;
00089   observers::NearestBackward nearest_backward_observer_;
00090   observers::AdjacentLeft adjacent_left_observer_;
00091   observers::AdjacentRight adjacent_right_observer_;
00092 
00093   boost::shared_ptr<tf::TransformListener> tf_listener_;
00094 
00095   // ROS topic subscriptions and publishers
00096   ros::Subscriber pc_sub_;              
00097   ros::Subscriber pc2_sub_;             
00098   ros::Subscriber road_map_sub_;
00099   ros::Subscriber odom_sub_;
00100   ros::Publisher observations_pub_;
00101   ros::Publisher viz_pub_;
00102 
00103   PtCloud obstacles_;                   
00104   art_msgs::ArtLanes local_map_;        
00105 
00107   std::vector<observers::Observer *> observers_;
00108 
00110   art_msgs::ObservationArray observations_;
00111 
00112   std::tr1::unordered_set<int> added_quads_; 
00113   art_msgs::ArtLanes obs_quads_;        
00114   std::vector<art_msgs::ArtQuadrilateral>::iterator obs_it_;
00115   art_msgs::ArtQuadrilateral robot_polygon_; 
00116   MapPose pose_; // pose of Map
00117 
00120   visualization_msgs::MarkerArray marks_msg_;
00121 };
00122 
00123 #endif // _LANE_OBSERVATIONS_H_


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