obstacles.cpp
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
37  *********************************************************************/
38 
40 #include <ros/console.h>
41 #include <ros/assert.h>
42 // #include <teb_local_planner/misc.h>
43 
44 namespace teb_local_planner
45 {
46 
47 
49 {
50  if (vertices_.size()<2)
51  return;
52 
53  if (vertices_.front().isApprox(vertices_.back()))
54  vertices_.pop_back();
55 }
56 
58 {
59  if (vertices_.empty())
60  {
61  centroid_.setConstant(NAN);
62  ROS_WARN("PolygonObstacle::calcCentroid(): number of vertices is empty. the resulting centroid is a vector of NANs.");
63  return;
64  }
65 
66  // if polygon is a point
67  if (noVertices()==1)
68  {
69  centroid_ = vertices_.front();
70  return;
71  }
72 
73  // if polygon is a line:
74  if (noVertices()==2)
75  {
76  centroid_ = 0.5*(vertices_.front() + vertices_.back());
77  return;
78  }
79 
80  // otherwise:
81 
82  centroid_.setZero();
83 
84  // calculate centroid (see wikipedia http://de.wikipedia.org/wiki/Geometrischer_Schwerpunkt#Polygon)
85  double A = 0; // A = 0.5 * sum_0_n-1 (x_i * y_{i+1} - x_{i+1} * y_i)
86  for (int i=0; i < noVertices()-1; ++i)
87  {
88  A += vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1);
89  }
90  A += vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1);
91  A *= 0.5;
92 
93  if (A!=0)
94  {
95  for (int i=0; i < noVertices()-1; ++i)
96  {
97  double aux = (vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1));
98  centroid_ += ( vertices_.at(i) + vertices_.at(i+1) )*aux;
99  }
100  double aux = (vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1));
101  centroid_ += ( vertices_.at(noVertices()-1) + vertices_.at(0) )*aux;
102  centroid_ /= (6*A);
103  }
104  else // A == 0 -> all points are placed on a 'perfect' line
105  {
106  // seach for the two outer points of the line with the maximum distance inbetween
107  int i_cand = 0;
108  int j_cand = 0;
109  double max_dist = 0;
110  for (int i=0; i< noVertices(); ++i)
111  {
112  for (int j=i+1; j< noVertices(); ++j) // start with j=i+1
113  {
114  double dist = (vertices_[j] - vertices_[i]).norm();
115  if (dist > max_dist)
116  {
117  max_dist = dist;
118  i_cand = i;
119  j_cand = j;
120  }
121  }
122  }
123  // calc centroid of that line
124  centroid_ = 0.5*(vertices_[i_cand] + vertices_[j_cand]);
125  }
126 }
127 
128 
129 
130 Eigen::Vector2d PolygonObstacle::getClosestPoint(const Eigen::Vector2d& position) const
131 {
132  // the polygon is a point
133  if (noVertices() == 1)
134  {
135  return vertices_.front();
136  }
137 
138  if (noVertices() > 1)
139  {
140 
141  Eigen::Vector2d new_pt = closest_point_on_line_segment_2d(position, vertices_.at(0), vertices_.at(1));
142 
143  if (noVertices() > 2) // real polygon and not a line
144  {
145  double dist = (new_pt-position).norm();
146  Eigen::Vector2d closest_pt = new_pt;
147 
148  // check each polygon edge
149  for (int i=1; i<noVertices()-1; ++i) // skip the first one, since we already checked it (new_pt)
150  {
151  new_pt = closest_point_on_line_segment_2d(position, vertices_.at(i), vertices_.at(i+1));
152  double new_dist = (new_pt-position).norm();
153  if (new_dist < dist)
154  {
155  dist = new_dist;
156  closest_pt = new_pt;
157  }
158  }
159  // afterwards we check the edge between goal and start (close polygon)
160  new_pt = closest_point_on_line_segment_2d(position, vertices_.back(), vertices_.front());
161  double new_dist = (new_pt-position).norm();
162  if (new_dist < dist)
163  return new_pt;
164  else
165  return closest_pt;
166  }
167  else
168  {
169  return new_pt; // closest point on line segment
170  }
171  }
172 
173  ROS_ERROR("PolygonObstacle::getClosestPoint() cannot find any closest point. Polygon ill-defined?");
174  return Eigen::Vector2d::Zero(); // todo: maybe boost::optional?
175 }
176 
177 
178 bool PolygonObstacle::checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist) const
179 {
180  // Simple strategy, check all edge-line intersections until an intersection is found...
181  // check each polygon edge
182  for (int i=0; i<noVertices()-1; ++i)
183  {
184  if ( check_line_segments_intersection_2d(line_start, line_end, vertices_.at(i), vertices_.at(i+1)) )
185  return true;
186  }
187  if (noVertices()==2) // if polygon is a line
188  return false;
189 
190  return check_line_segments_intersection_2d(line_start, line_end, vertices_.back(), vertices_.front()); //otherwise close polygon
191 }
192 
193 
194 
195 // implements toPolygonMsg() of the base class
196 void PolygonObstacle::toPolygonMsg(geometry_msgs::Polygon& polygon)
197 {
198  polygon.points.resize(vertices_.size());
199  for (std::size_t i=0; i<vertices_.size(); ++i)
200  {
201  polygon.points[i].x = vertices_[i].x();
202  polygon.points[i].y = vertices_[i].y();
203  polygon.points[i].z = 0;
204  }
205 }
206 
207 
208 
209 
210 
211 
212 
213 
214 } // namespace teb_local_planner
bool check_line_segments_intersection_2d(const Eigen::Ref< const Eigen::Vector2d > &line1_start, const Eigen::Ref< const Eigen::Vector2d > &line1_end, const Eigen::Ref< const Eigen::Vector2d > &line2_start, const Eigen::Ref< const Eigen::Vector2d > &line2_end, Eigen::Vector2d *intersection=NULL)
Helper function to check whether two line segments intersects.
int noVertices() const
Get the number of vertices defining the polygon (the first vertex is counted once) ...
Definition: obstacles.h:941
virtual void toPolygonMsg(geometry_msgs::Polygon &polygon)
Convert the obstacle to a polygon message.
Definition: obstacles.cpp:196
Point2dContainer vertices_
Store vertices defining the polygon (.
Definition: obstacles.h:953
#define ROS_WARN(...)
virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d &position) const
Get the closest point on the boundary of the obstacle w.r.t. a specified reference position...
Definition: obstacles.cpp:130
void fixPolygonClosure()
Check if the current polygon contains the first vertex twice (as start and end) and in that case eras...
Definition: obstacles.cpp:48
Eigen::Vector2d closest_point_on_line_segment_2d(const Eigen::Ref< const Eigen::Vector2d > &point, const Eigen::Ref< const Eigen::Vector2d > &line_start, const Eigen::Ref< const Eigen::Vector2d > &line_end)
Helper function to obtain the closest point on a line segment w.r.t. a reference point.
virtual bool checkLineIntersection(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double min_dist=0) const
Check if a given line segment between two points intersects with the obstacle (and additionally keeps...
Definition: obstacles.cpp:178
void calcCentroid()
Compute the centroid of the polygon (called inside finalizePolygon())
Definition: obstacles.cpp:57
Eigen::Vector2d centroid_
Store the centroid coordinates of the polygon (.
Definition: obstacles.h:954
#define ROS_ERROR(...)


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10