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_