costmap_to_polygons.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Christoph Rösmann, Otniel Rinaldo
37  *********************************************************************/
38 
39 #ifndef COSTMAP_TO_POLYGONS_H_
40 #define COSTMAP_TO_POLYGONS_H_
41 
42 #include <ros/ros.h>
44 #include <nav_msgs/OccupancyGrid.h>
45 #include <visualization_msgs/Marker.h>
46 #include <geometry_msgs/Point.h>
47 #include <geometry_msgs/Polygon.h>
48 #include <vector>
49 #include <algorithm>
50 #include <Eigen/Core>
51 #include <Eigen/StdVector>
52 #include <boost/thread/mutex.hpp>
53 #include <boost/thread/thread.hpp>
54 
55 // dynamic reconfigure
56 #include <costmap_converter/CostmapToPolygonsDBSMCCHConfig.h>
57 #include <dynamic_reconfigure/server.h>
58 
59 
60 namespace costmap_converter
61 {
62 
80 {
81  public:
82 
87  struct KeyPoint
88  {
90  KeyPoint() {}
92  KeyPoint(double x_, double y_) : x(x_), y(y_) {}
93  double x;
94  double y;
95 
97  void toPointMsg(geometry_msgs::Point& point) const {point.x=x; point.y=y; point.z=0;}
99  void toPointMsg(geometry_msgs::Point32& point) const {point.x=x; point.y=y; point.z=0;}
100  };
101 
106  struct Parameters
107  {
108  Parameters() : max_distance_(0.4), min_pts_(2), max_pts_(30), min_keypoint_separation_(0.1) {}
109  // DBSCAN parameters
110  double max_distance_;
111  int min_pts_;
112  int max_pts_;
113 
114  // convex hull parameters
116  };
117 
122 
126  virtual ~CostmapToPolygonsDBSMCCH();
127 
132  virtual void initialize(ros::NodeHandle nh);
133 
137  virtual void compute();
138 
144  virtual void setCostmap2D(costmap_2d::Costmap2D* costmap);
145 
150  virtual void updateCostmap2D();
151 
152 
159  template< typename Point>
160  static void convertPointToPolygon(const Point& point, geometry_msgs::Polygon& polygon)
161  {
162  polygon.points.resize(1);
163  polygon.points.front().x = point.x;
164  polygon.points.front().y = point.y;
165  polygon.points.front().z = 0;
166  }
167 
174 
175 
176 
177  protected:
178 
190  void dbScan(std::vector< std::vector<KeyPoint> >& clusters);
191 
198  void regionQuery(int curr_index, std::vector<int>& neighbor_indices);
199 
203  void addPoint(double x, double y)
204  {
205  int idx = occupied_cells_.size();
206  occupied_cells_.emplace_back(x, y);
207  int cx, cy;
208  pointToNeighborCells(occupied_cells_.back(), cx, cy);
209  int nidx = neighborCellsToIndex(cx, cy);
210  if (nidx >= 0)
211  neighbor_lookup_[nidx].push_back(idx);
212  }
213 
226  void convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);
227 
241  void convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon);
242 
250  void simplifyPolygon(geometry_msgs::Polygon& polygon);
251 
261  template <typename P1, typename P2, typename P3>
262  long double cross(const P1& O, const P2& A, const P3& B)
263  {
264  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);
265  }
266 
267 
273 
274 
275  std::vector<KeyPoint> occupied_cells_;
276 
277  std::vector<std::vector<int> > neighbor_lookup_;
280  double offset_x_;
281  double offset_y_;
282 
288  int neighborCellsToIndex(int cx, int cy)
289  {
290  if (cx < 0 || cx >= neighbor_size_x_ || cy < 0 || cy >= neighbor_size_y_)
291  return -1;
292  return cy * neighbor_size_x_ + cx;
293  }
294 
301  void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy)
302  {
303  cx = int((kp.x - offset_x_) / parameter_.max_distance_);
304  cy = int((kp.y - offset_y_) / parameter_.max_distance_);
305  }
306 
307 
308  Parameters parameter_; //< active parameters throughout computation
309  Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure
310  boost::mutex parameter_mutex_;
311 
312  private:
313 
321  void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level);
322 
323 
325  boost::mutex mutex_;
326 
327  dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>* dynamic_recfg_;
328 
330 
331 };
332 
333 
334 } //end namespace teb_local_planner
335 
336 #endif /* COSTMAP_TO_POLYGONS_H_ */
virtual void updateCostmap2D()
Get updated data from the previously set Costmap2D.
void reconfigureCB(CostmapToPolygonsDBSMCCHConfig &config, uint32_t level)
Callback for the dynamic_reconfigure node.
std::vector< KeyPoint > occupied_cells_
List of occupied cells in the current map (updated by updateCostmap2D())
double min_keypoint_separation_
Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)...
void pointToNeighborCells(const KeyPoint &kp, int &cx, int &cy)
compute the cell indices of a keypoint
void toPointMsg(geometry_msgs::Point32 &point) const
Convert keypoint to geometry_msgs::Point32 message type.
static void convertPointToPolygon(const Point &point, geometry_msgs::Polygon &polygon)
Convert a generi point type to a geometry_msgs::Polygon.
void dbScan(std::vector< std::vector< KeyPoint > > &clusters)
DBSCAN algorithm for clustering.
double offset_y_
offset [meters] in x for the lookup grid
void simplifyPolygon(geometry_msgs::Polygon &polygon)
Simplify a polygon by removing points.
void addPoint(double x, double y)
helper function for adding a point to the lookup data structures
virtual void compute()
This method performs the actual work (conversion of the costmap to polygons)
int max_pts_
Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes...
int neighbor_size_y_
size of the neighbour lookup in x (number of cells)
virtual void setCostmap2D(costmap_2d::Costmap2D *costmap)
Pass a pointer to the costap to the plugin.
TFSIMD_FORCE_INLINE const tfScalar & x() const
costmap_2d::Costmap2D * costmap_
Pointer to the costmap2d.
int neighbor_size_x_
array of cells for neighbor lookup
int min_pts_
Parameter for DB_Scan: minimum number of points that define a cluster.
boost::mutex parameter_mutex_
Mutex that keeps track about the ownership of the shared polygon instance.
double offset_x_
size of the neighbour lookup in y (number of cells)
double max_distance_
Parameter for DB_Scan, maximum distance to neighbors [m].
This abstract class defines the interface for plugins that convert the costmap into polygon types...
TFSIMD_FORCE_INLINE const tfScalar & y() const
dynamic_reconfigure::Server< CostmapToPolygonsDBSMCCHConfig > * dynamic_recfg_
Dynamic reconfigure server to allow config modifications at runtime.
This class converts the costmap_2d into a set of convex polygons (and points)
PolygonContainerPtr polygons_
Current shared container of polygons.
void regionQuery(int curr_index, std::vector< int > &neighbor_indices)
Helper function for dbScan to search for neighboring points.
KeyPoint(double x_, double y_)
Constructor with point initialization.
void convexHull2(std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
Compute the convex hull for a single cluster.
boost::mutex mutex_
Mutex that keeps track about the ownership of the shared polygon instance.
int neighborCellsToIndex(int cx, int cy)
offset [meters] in y for the lookup grid
Defines a keypoint in metric coordinates of the map.
void updatePolygonContainer(PolygonContainerPtr polygons)
Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside...
void convexHull(std::vector< KeyPoint > &cluster, geometry_msgs::Polygon &polygon)
Compute the convex hull for a single cluster (monotone chain algorithm)
PolygonContainerConstPtr getPolygons()
Get a shared instance of the current polygon container.
std::vector< std::vector< int > > neighbor_lookup_
void toPointMsg(geometry_msgs::Point &point) const
Convert keypoint to geometry_msgs::Point message type.
virtual void initialize(ros::NodeHandle nh)
Initialize the plugin.
long double cross(const P1 &O, const P2 &A, const P3 &B)
2D Cross product of two vectors defined by two points and a common origin


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat May 16 2020 03:19:18