QuadrilateralOps.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: QuadrilateralOps.h 1681 2011-08-18 20:37:44Z ruifei0713 $
00007  */
00008 
00019 #ifndef __QUADRILATERAL_OPS_H__
00020 #define __QUADRILATERAL_OPS_H__
00021 
00022 #include <art_msgs/ArtQuadrilateral.h>
00023 #include <art_msgs/ArtLanes.h>
00024 #include <art_map/PolyOps.h>
00025 #include <nav_msgs/Odometry.h>
00026 
00027 namespace quad_ops 
00028 {
00030   typedef art_msgs::ArtQuadrilateral Quad;
00031 
00032   bool pointInHull(float x, float y,
00033                    const geometry_msgs::Point32 *p1,
00034                    const geometry_msgs::Point32 *p2,
00035                    const geometry_msgs::Point32 *p3,
00036                    const geometry_msgs::Point32 *p4);  
00037 
00038   bool quickPointInPoly(float x, float y, const Quad& p);
00039   
00040   bool quickPointInPolyRatio(float x, float y, const Quad& p, float ratio);
00041 
00042   art_msgs::ArtLanes filterLanes(const Quad& base_quad,
00043                                  const art_msgs::ArtLanes& quads,
00044                                  bool(*filter)(const Quad&, const Quad&));
00045   art_msgs::ArtLanes filterAdjacentLanes(MapPose &pose,
00046                                  const art_msgs::ArtLanes& quads,
00047                                  const int lane);
00048   // Create a comparison operator so we can use ArtQuadrilateral's in
00049   // std::set or std::sort
00050   struct quad_less
00051   {
00052     bool operator()(const Quad& x, const Quad& y) const
00053     {
00054       return x.poly_id < y.poly_id;
00055     };
00056   };
00057 
00058   // Create some comparison operators for filtering ArtQuadrilaterals
00059   inline bool compare_seg_lane(const Quad& base, const Quad& comp)
00060   {
00061     return ( (base.start_way.seg == comp.start_way.seg) && 
00062              (base.start_way.lane == comp.start_way.lane) &&
00063              (base.end_way.seg == comp.end_way.seg) && 
00064              (base.end_way.lane == comp.end_way.lane) &&
00065              (!comp.is_transition) );
00066   };
00067 
00068   inline bool compare_forward_seg_lane(const Quad& base, const Quad& comp)
00069   {
00070     return ( (comp.poly_id > base.poly_id) &&  
00071              (base.start_way.seg == comp.start_way.seg) && 
00072              (base.start_way.lane == comp.start_way.lane) &&
00073              (base.end_way.seg == comp.end_way.seg) && 
00074              (base.end_way.lane == comp.end_way.lane) &&
00075              (!comp.is_transition) );
00076   };
00077 
00078   inline bool compare_backward_seg_lane(const Quad& base, const Quad& comp)
00079   {
00080     return ( (base.poly_id > comp.poly_id) && 
00081              (base.start_way.seg == comp.start_way.seg) && 
00082              (base.start_way.lane == comp.start_way.lane) &&
00083              (base.end_way.seg == comp.end_way.seg) && 
00084              (base.end_way.lane == comp.end_way.lane) &&
00085              (!comp.is_transition) );
00086   };
00087 }
00088 
00089 #endif // __QUADRILATERAL_OPS_H__


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