costmap_to_polygons.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann, Otniel Rinaldo
00037  *********************************************************************/
00038 
00039 #ifndef COSTMAP_TO_POLYGONS_H_
00040 #define COSTMAP_TO_POLYGONS_H_
00041 
00042 #include <ros/ros.h>
00043 #include <costmap_converter/costmap_converter_interface.h>
00044 #include <nav_msgs/OccupancyGrid.h>
00045 #include <visualization_msgs/Marker.h>
00046 #include <geometry_msgs/Point.h>
00047 #include <geometry_msgs/Polygon.h>
00048 #include <vector>
00049 #include <algorithm>
00050 #include <Eigen/Core>
00051 #include <Eigen/StdVector>
00052 #include <boost/thread/mutex.hpp>
00053 #include <boost/thread/thread.hpp>
00054 
00055 // dynamic reconfigure
00056 #include <costmap_converter/CostmapToPolygonsDBSMCCHConfig.h>
00057 #include <dynamic_reconfigure/server.h>
00058 
00059 
00060 namespace costmap_converter
00061 {
00062   
00079 class CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons
00080 {
00081   public:
00082     
00087     struct KeyPoint
00088     {
00090       KeyPoint() {}
00092       KeyPoint(double x_, double y_) : x(x_), y(y_) {}
00093       double x; 
00094       double y; 
00095       
00097       void toPointMsg(geometry_msgs::Point& point) const {point.x=x; point.y=y; point.z=0;}
00099       void toPointMsg(geometry_msgs::Point32& point) const {point.x=x; point.y=y; point.z=0;}
00100     };
00101     
00102     
00106     CostmapToPolygonsDBSMCCH();
00107     
00111     virtual ~CostmapToPolygonsDBSMCCH();
00112     
00117     virtual void initialize(ros::NodeHandle nh);
00118     
00122     virtual void compute();
00123         
00129     virtual void setCostmap2D(costmap_2d::Costmap2D* costmap);
00130     
00135     virtual void updateCostmap2D();
00136     
00137     
00144     template< typename Point>
00145     static void convertPointToPolygon(const Point& point, geometry_msgs::Polygon& polygon)
00146     {
00147       polygon.points.resize(1);
00148       polygon.points.front().x = point.x;
00149       polygon.points.front().y = point.y;
00150       polygon.points.front().z = 0;
00151     }
00152     
00158     PolygonContainerConstPtr getPolygons();
00159 
00160     
00161     
00162   protected:
00163     
00176     void dbScan(const std::vector<KeyPoint>& occupied_cells, std::vector< std::vector<KeyPoint> >& clusters);
00177     
00185     void regionQuery(const std::vector<KeyPoint>& occupied_cells, int curr_index, std::vector<int>& neighbor_indices);
00186 
00187     
00200     void convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);
00201     
00215     void convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);
00216     
00226     template <typename P1, typename P2, typename P3>
00227     long double cross(const P1& O, const P2& A, const P3& B)
00228     { 
00229         return (long double)(A.x - O.x) * (long double)(B.y - O.y) - (long double)(A.y - O.y) * (long double)(B.x - O.x);
00230     }
00231     
00232       
00237    void updatePolygonContainer(PolygonContainerPtr polygons);   
00238           
00239    
00240    std::vector<KeyPoint> occupied_cells_; 
00241 
00242    // DBSCAN parameters
00243    double max_distance_; 
00244    int min_pts_; 
00245    int max_pts_; 
00246    
00247    // convex hull parameters
00248    double min_keypoint_separation_; 
00249    
00250   private:
00251        
00259     void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level);
00260     
00261     
00262     PolygonContainerPtr polygons_; 
00263     boost::mutex mutex_; 
00264     
00265     dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* dynamic_recfg_; 
00266    
00267     costmap_2d::Costmap2D *costmap_; 
00268    
00269 }; 
00270 
00271   
00272 } //end namespace teb_local_planner
00273 
00274 #endif /* COSTMAP_TO_POLYGONS_H_ */


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Wed Sep 20 2017 03:00:37