#include <lane_observations.h>
Public Member Functions | |
| LaneObservations (ros::NodeHandle &node, ros::NodeHandle &priv_nh) | |
| Run lane observers, publish their observations.   | |
| void | spin () | 
| Handle incoming data.   | |
| ~LaneObservations () | |
| Deconstructor.   | |
Private Types | |
| typedef pcl::PointCloud < pcl::PointXYZ >  | PtCloud | 
| short name for PCL PointCloud   | |
Private Member Functions | |
| void | addObserver (observers::Observer &obs) | 
| Add an entry to the observer vector.   | |
| void | calcRobotPolygon () | 
| Calculate which polygon currently contains the robot.   | |
| void | filterPointsInLocalMap () | 
| Filter obstacle points to those in a road map polygon.   | |
| bool | isPointInAPolygon (float x, float y) | 
| void | processLocalMap (const art_msgs::ArtLanes::ConstPtr &msg) | 
| Local road map callback.   | |
| void | processObstacles (void) | 
| Obstacles point cloud processing. Starts all the observers.   | |
| void | processPointCloud (const sensor_msgs::PointCloud::ConstPtr &msg) | 
| PointCloud callback.   | |
| void | processPointCloud2 (const sensor_msgs::PointCloud2::ConstPtr &msg) | 
| PointCloud2 callback.   | |
| void | processPose (const nav_msgs::Odometry &odom) | 
| process the pose of the map   | |
| void | publishObstacleVisualization () | 
| Publish rviz markers for obstacles in the road.   | |
| void | runObservers () | 
| Run all registered observers and publish their observations.   | |
| void | transformPointCloud (const PtCloud &msg) | 
| Transform obstacle points into the map frame of reference.   | |
Private Attributes | |
| std::tr1::unordered_set< int > | added_quads_ | 
| set of obstacle quads   | |
| observers::AdjacentLeft | adjacent_left_observer_ | 
| observers::AdjacentRight | adjacent_right_observer_ | 
| Config | config_ | 
| configuration parameters   | |
| art_msgs::ArtLanes | local_map_ | 
| local road map   | |
| visualization_msgs::MarkerArray | marks_msg_ | 
| observers::NearestBackward | nearest_backward_observer_ | 
| observers::NearestForward | nearest_forward_observer_ | 
| ros::NodeHandle | node_ | 
| node handle   | |
| std::vector < art_msgs::ArtQuadrilateral > ::iterator  | obs_it_ | 
| art_msgs::ArtLanes | obs_quads_ | 
| vector of obstacle quads   | |
| art_msgs::ObservationArray | observations_ | 
| current observations from the observers   | |
| ros::Publisher | observations_pub_ | 
| std::vector < observers::Observer * >  | observers_ | 
| vector of observers, in order of the observations they publish   | |
| PtCloud | obstacles_ | 
| current obstacle data   | |
| ros::Subscriber | odom_sub_ | 
| ros::Subscriber | pc2_sub_ | 
| PointCloud2 input.   | |
| ros::Subscriber | pc_sub_ | 
| deprecated PointCloud input   | |
| MapPose | pose_ | 
| ros::NodeHandle | priv_nh_ | 
| private node handle   | |
| ros::Subscriber | road_map_sub_ | 
| art_msgs::ArtQuadrilateral | robot_polygon_ | 
| robot's current polygon   | |
| boost::shared_ptr < tf::TransformListener >  | tf_listener_ | 
| ros::Publisher | viz_pub_ | 
Definition at line 46 of file lane_observations.h.
typedef pcl::PointCloud<pcl::PointXYZ> LaneObservations::PtCloud [private] | 
        
short name for PCL PointCloud
Definition at line 58 of file lane_observations.h.
| LaneObservations::LaneObservations | ( | ros::NodeHandle & | node, | 
| ros::NodeHandle & | priv_nh | ||
| ) | 
Run lane observers, publish their observations.
Definition at line 26 of file lane_observations.cc.
Deconstructor.
Definition at line 74 of file lane_observations.cc.
| void LaneObservations::addObserver | ( | observers::Observer & | obs | ) |  [inline, private] | 
        
Add an entry to the observer vector.
| obs | observer to add | 
Definition at line 64 of file lane_observations.h.
| void LaneObservations::calcRobotPolygon | ( | ) |  [private] | 
        
Calculate which polygon currently contains the robot.
Definition at line 214 of file lane_observations.cc.
| void LaneObservations::filterPointsInLocalMap | ( | ) |  [private] | 
        
Filter obstacle points to those in a road map polygon.
Definition at line 127 of file lane_observations.cc.
| bool LaneObservations::isPointInAPolygon | ( | float | x, | 
| float | y | ||
| ) |  [private] | 
        
Point in polygon predicate.
Definition at line 168 of file lane_observations.cc.
| void LaneObservations::processLocalMap | ( | const art_msgs::ArtLanes::ConstPtr & | msg | ) |  [private] | 
        
Local road map callback.
Definition at line 115 of file lane_observations.cc.
| void LaneObservations::processObstacles | ( | void | ) |  [private] | 
        
Obstacles point cloud processing. Starts all the observers.
obstacles_ contains the point cloud data received Definition at line 80 of file lane_observations.cc.
| void LaneObservations::processPointCloud | ( | const sensor_msgs::PointCloud::ConstPtr & | msg | ) |  [private] | 
        
PointCloud callback.
Definition at line 96 of file lane_observations.cc.
| void LaneObservations::processPointCloud2 | ( | const sensor_msgs::PointCloud2::ConstPtr & | msg | ) |  [private] | 
        
PointCloud2 callback.
Definition at line 105 of file lane_observations.cc.
| void LaneObservations::processPose | ( | const nav_msgs::Odometry & | odom | ) |  [private] | 
        
process the pose of the map
Definition at line 121 of file lane_observations.cc.
| void LaneObservations::publishObstacleVisualization | ( | ) |  [private] | 
        
Publish rviz markers for obstacles in the road.
Definition at line 250 of file lane_observations.cc.
| void LaneObservations::runObservers | ( | ) |  [private] | 
        
Run all registered observers and publish their observations.
Definition at line 200 of file lane_observations.cc.
| void LaneObservations::spin | ( | void | ) | 
Handle incoming data.
Definition at line 323 of file lane_observations.cc.
| void LaneObservations::transformPointCloud | ( | const PtCloud & | msg | ) |  [private] | 
        
Transform obstacle points into the map frame of reference.
Definition at line 140 of file lane_observations.cc.
std::tr1::unordered_set<int> LaneObservations::added_quads_ [private] | 
        
set of obstacle quads
Definition at line 112 of file lane_observations.h.
Definition at line 90 of file lane_observations.h.
Definition at line 91 of file lane_observations.h.
Config LaneObservations::config_ [private] | 
        
configuration parameters
Definition at line 85 of file lane_observations.h.
local road map
Definition at line 104 of file lane_observations.h.
visualization_msgs::MarkerArray LaneObservations::marks_msg_ [private] | 
        
Only used within LaneObservations::publishObstacleVisualization(), a class variable only to avoid memory allocation on every cycle.
Definition at line 120 of file lane_observations.h.
Definition at line 89 of file lane_observations.h.
Definition at line 88 of file lane_observations.h.
ros::NodeHandle LaneObservations::node_ [private] | 
        
node handle
Definition at line 82 of file lane_observations.h.
std::vector<art_msgs::ArtQuadrilateral>::iterator LaneObservations::obs_it_ [private] | 
        
Definition at line 114 of file lane_observations.h.
vector of obstacle quads
Definition at line 113 of file lane_observations.h.
current observations from the observers
Definition at line 110 of file lane_observations.h.
Definition at line 100 of file lane_observations.h.
std::vector<observers::Observer *> LaneObservations::observers_ [private] | 
        
vector of observers, in order of the observations they publish
Definition at line 107 of file lane_observations.h.
PtCloud LaneObservations::obstacles_ [private] | 
        
current obstacle data
Definition at line 103 of file lane_observations.h.
ros::Subscriber LaneObservations::odom_sub_ [private] | 
        
Definition at line 99 of file lane_observations.h.
ros::Subscriber LaneObservations::pc2_sub_ [private] | 
        
PointCloud2 input.
Definition at line 97 of file lane_observations.h.
ros::Subscriber LaneObservations::pc_sub_ [private] | 
        
deprecated PointCloud input
Definition at line 96 of file lane_observations.h.
MapPose LaneObservations::pose_ [private] | 
        
Definition at line 116 of file lane_observations.h.
ros::NodeHandle LaneObservations::priv_nh_ [private] | 
        
private node handle
Definition at line 83 of file lane_observations.h.
Definition at line 98 of file lane_observations.h.
robot's current polygon
Definition at line 115 of file lane_observations.h.
boost::shared_ptr<tf::TransformListener> LaneObservations::tf_listener_ [private] | 
        
Definition at line 93 of file lane_observations.h.
ros::Publisher LaneObservations::viz_pub_ [private] | 
        
Definition at line 101 of file lane_observations.h.