obstacles.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
37  *********************************************************************/
38 
39 
40 #ifndef OBSTACLES_H
41 #define OBSTACLES_H
42 
43 #include <Eigen/Core>
44 #include <Eigen/StdVector>
45 #include <Eigen/Geometry>
46 
47 #include <complex>
48 
49 #include <boost/shared_ptr.hpp>
50 #include <boost/pointer_cast.hpp>
51 
52 #include <geometry_msgs/Polygon.h>
53 #include <geometry_msgs/TwistWithCovariance.h>
54 #include <geometry_msgs/QuaternionStamped.h>
55 
56 #include <tf/tf.h>
58 
59 
60 namespace teb_local_planner
61 {
62 
67 class Obstacle
68 {
69 public:
70 
74  Obstacle() : dynamic_(false), centroid_velocity_(Eigen::Vector2d::Zero())
75  {
76  }
77 
81  virtual ~Obstacle()
82  {
83  }
84 
85 
88 
93  virtual const Eigen::Vector2d& getCentroid() const = 0;
94 
99  virtual std::complex<double> getCentroidCplx() const = 0;
100 
102 
103 
106 
113  virtual bool checkCollision(const Eigen::Vector2d& position, double min_dist) const = 0;
114 
122  virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const = 0;
123 
129  virtual double getMinimumDistance(const Eigen::Vector2d& position) const = 0;
130 
137  virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const = 0;
138 
144  virtual double getMinimumDistance(const Point2dContainer& polygon) const = 0;
145 
151  virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const = 0;
152 
154 
155 
156 
159 
166  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const = 0;
167 
175  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const = 0;
176 
183  virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const = 0;
184 
190  virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
191  {
192  position = getCentroid() + t * getCentroidVelocity();
193  }
194 
199  bool isDynamic() const {return dynamic_;}
200 
206  void setCentroidVelocity(const Eigen::Ref<const Eigen::Vector2d>& vel) {centroid_velocity_ = vel; dynamic_=true;}
207 
214  void setCentroidVelocity(const geometry_msgs::TwistWithCovariance& velocity,
215  const geometry_msgs::Quaternion& orientation)
216  {
217  // Set velocity, if obstacle is moving
218  Eigen::Vector2d vel;
219  vel.coeffRef(0) = velocity.twist.linear.x;
220  vel.coeffRef(1) = velocity.twist.linear.y;
221 
222  // If norm of velocity is less than 0.001, consider obstacle as not dynamic
223  // TODO: Get rid of constant
224  if (vel.norm() < 0.001)
225  return;
226 
227  // currently velocity published by stage is already given in the map frame
228 // double yaw = tf::getYaw(orientation.quaternion);
229 // ROS_INFO("Yaw: %f", yaw);
230 // Eigen::Rotation2Dd rot(yaw);
231 // vel = rot * vel;
232  setCentroidVelocity(vel);
233  }
234 
235  void setCentroidVelocity(const geometry_msgs::TwistWithCovariance& velocity,
236  const geometry_msgs::QuaternionStamped& orientation)
237  {
238  setCentroidVelocity(velocity, orientation.quaternion);
239  }
240 
245  const Eigen::Vector2d& getCentroidVelocity() const {return centroid_velocity_;}
246 
248 
249 
250 
253 
262  virtual void toPolygonMsg(geometry_msgs::Polygon& polygon) = 0;
263 
264  virtual void toTwistWithCovarianceMsg(geometry_msgs::TwistWithCovariance& twistWithCovariance)
265  {
266  if (dynamic_)
267  {
268  twistWithCovariance.twist.linear.x = centroid_velocity_(0);
269  twistWithCovariance.twist.linear.y = centroid_velocity_(1);
270  }
271  else
272  {
273  twistWithCovariance.twist.linear.x = 0;
274  twistWithCovariance.twist.linear.y = 0;
275  }
276 
277  // TODO:Covariance
278  }
279 
281 
282 protected:
283 
284  bool dynamic_;
285  Eigen::Vector2d centroid_velocity_;
286 
287 public:
288  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
289 };
290 
291 
297 typedef std::vector<ObstaclePtr> ObstContainer;
298 
299 
300 
305 class PointObstacle : public Obstacle
306 {
307 public:
308 
312  PointObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero())
313  {}
314 
319  PointObstacle(const Eigen::Ref< const Eigen::Vector2d>& position) : Obstacle(), pos_(position)
320  {}
321 
327  PointObstacle(double x, double y) : Obstacle(), pos_(Eigen::Vector2d(x,y))
328  {}
329 
330 
331  // implements checkCollision() of the base class
332  virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
333  {
334  return getMinimumDistance(point) < min_dist;
335  }
336 
337 
338  // implements checkLineIntersection() of the base class
339  virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
340  {
341  // Distance Line - Circle
342  // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
343  Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x
344  Eigen::Vector2d b = pos_-line_start; // b=m-x
345 
346  // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
347  double t = a.dot(b)/a.dot(a);
348  if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
349  else if (t>1) t=1;
350  Eigen::Vector2d nearest_point = line_start + a*t;
351 
352  // check collision
353  return checkCollision(nearest_point, min_dist);
354  }
355 
356 
357  // implements getMinimumDistance() of the base class
358  virtual double getMinimumDistance(const Eigen::Vector2d& position) const
359  {
360  return (position-pos_).norm();
361  }
362 
363  // implements getMinimumDistance() of the base class
364  virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
365  {
366  return distance_point_to_segment_2d(pos_, line_start, line_end);
367  }
368 
369  // implements getMinimumDistance() of the base class
370  virtual double getMinimumDistance(const Point2dContainer& polygon) const
371  {
372  return distance_point_to_polygon_2d(pos_, polygon);
373  }
374 
375  // implements getMinimumDistanceVec() of the base class
376  virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
377  {
378  return pos_;
379  }
380 
381  // implements getMinimumSpatioTemporalDistance() of the base class
382  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
383  {
384  return (pos_ + t*centroid_velocity_ - position).norm();
385  }
386 
387  // implements getMinimumSpatioTemporalDistance() of the base class
388  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
389  {
390  return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end);
391  }
392 
393  // implements getMinimumSpatioTemporalDistance() of the base class
394  virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
395  {
396  return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon);
397  }
398 
399  // implements predictCentroidConstantVelocity() of the base class
400  virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
401  {
402  position = pos_ + t*centroid_velocity_;
403  }
404 
405  // implements getCentroid() of the base class
406  virtual const Eigen::Vector2d& getCentroid() const
407  {
408  return pos_;
409  }
410 
411  // implements getCentroidCplx() of the base class
412  virtual std::complex<double> getCentroidCplx() const
413  {
414  return std::complex<double>(pos_[0],pos_[1]);
415  }
416 
417  // Accessor methods
418  const Eigen::Vector2d& position() const {return pos_;}
419  Eigen::Vector2d& position() {return pos_;}
420  double& x() {return pos_.coeffRef(0);}
421  const double& x() const {return pos_.coeffRef(0);}
422  double& y() {return pos_.coeffRef(1);}
423  const double& y() const {return pos_.coeffRef(1);}
424 
425  // implements toPolygonMsg() of the base class
426  virtual void toPolygonMsg(geometry_msgs::Polygon& polygon)
427  {
428  polygon.points.resize(1);
429  polygon.points.front().x = pos_.x();
430  polygon.points.front().y = pos_.y();
431  polygon.points.front().z = 0;
432  }
433 
434 protected:
435 
436  Eigen::Vector2d pos_;
437 
438 
439 public:
440  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
441 };
442 
448 {
449 public:
450 
454  CircularObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero())
455  {}
456 
462  CircularObstacle(const Eigen::Ref< const Eigen::Vector2d>& position, double radius) : Obstacle(), pos_(position), radius_(radius)
463  {}
464 
471  CircularObstacle(double x, double y, double radius) : Obstacle(), pos_(Eigen::Vector2d(x,y)), radius_(radius)
472  {}
473 
474 
475  // implements checkCollision() of the base class
476  virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
477  {
478  return getMinimumDistance(point) < min_dist;
479  }
480 
481 
482  // implements checkLineIntersection() of the base class
483  virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
484  {
485  // Distance Line - Circle
486  // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke
487  Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x
488  Eigen::Vector2d b = pos_-line_start; // b=m-x
489 
490  // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1
491  double t = a.dot(b)/a.dot(a);
492  if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line
493  else if (t>1) t=1;
494  Eigen::Vector2d nearest_point = line_start + a*t;
495 
496  // check collision
497  return checkCollision(nearest_point, min_dist);
498  }
499 
500 
501  // implements getMinimumDistance() of the base class
502  virtual double getMinimumDistance(const Eigen::Vector2d& position) const
503  {
504  return (position-pos_).norm() - radius_;
505  }
506 
507  // implements getMinimumDistance() of the base class
508  virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
509  {
510  return distance_point_to_segment_2d(pos_, line_start, line_end) - radius_;
511  }
512 
513  // implements getMinimumDistance() of the base class
514  virtual double getMinimumDistance(const Point2dContainer& polygon) const
515  {
516  return distance_point_to_polygon_2d(pos_, polygon) - radius_;
517  }
518 
519  // implements getMinimumDistanceVec() of the base class
520  virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
521  {
522  return pos_ + radius_*(position-pos_).normalized();
523  }
524 
525  // implements getMinimumSpatioTemporalDistance() of the base class
526  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
527  {
528  return (pos_ + t*centroid_velocity_ - position).norm() - radius_;
529  }
530 
531  // implements getMinimumSpatioTemporalDistance() of the base class
532  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
533  {
534  return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end) - radius_;
535  }
536 
537  // implements getMinimumSpatioTemporalDistance() of the base class
538  virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
539  {
540  return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon) - radius_;
541  }
542 
543  // implements predictCentroidConstantVelocity() of the base class
544  virtual void predictCentroidConstantVelocity(double t, Eigen::Ref<Eigen::Vector2d> position) const
545  {
546  position = pos_ + t*centroid_velocity_;
547  }
548 
549  // implements getCentroid() of the base class
550  virtual const Eigen::Vector2d& getCentroid() const
551  {
552  return pos_;
553  }
554 
555  // implements getCentroidCplx() of the base class
556  virtual std::complex<double> getCentroidCplx() const
557  {
558  return std::complex<double>(pos_[0],pos_[1]);
559  }
560 
561  // Accessor methods
562  const Eigen::Vector2d& position() const {return pos_;}
563  Eigen::Vector2d& position() {return pos_;}
564  double& x() {return pos_.coeffRef(0);}
565  const double& x() const {return pos_.coeffRef(0);}
566  double& y() {return pos_.coeffRef(1);}
567  const double& y() const {return pos_.coeffRef(1);}
568  double& radius() {return radius_;}
569  const double& radius() const {return radius_;}
570 
571  // implements toPolygonMsg() of the base class
572  virtual void toPolygonMsg(geometry_msgs::Polygon& polygon)
573  {
574  // TODO(roesmann): the polygon message type cannot describe a "perfect" circle
575  // We could switch to ObstacleMsg if required somewhere...
576  polygon.points.resize(1);
577  polygon.points.front().x = pos_.x();
578  polygon.points.front().y = pos_.y();
579  polygon.points.front().z = 0;
580  }
581 
582 protected:
583 
584  Eigen::Vector2d pos_;
585  double radius_ = 0.0;
586 
587 
588 public:
589  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
590 };
591 
597 class LineObstacle : public Obstacle
598 {
599 public:
601  typedef std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > VertexContainer;
602 
607  {
608  start_.setZero();
609  end_.setZero();
610  centroid_.setZero();
611  }
612 
618  LineObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end)
619  : Obstacle(), start_(line_start), end_(line_end)
620  {
621  calcCentroid();
622  }
623 
631  LineObstacle(double x1, double y1, double x2, double y2) : Obstacle()
632  {
633  start_.x() = x1;
634  start_.y() = y1;
635  end_.x() = x2;
636  end_.y() = y2;
637  calcCentroid();
638  }
639 
640  // implements checkCollision() of the base class
641  virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
642  {
643  return getMinimumDistance(point) <= min_dist;
644  }
645 
646  // implements checkLineIntersection() of the base class
647  virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const
648  {
649  return check_line_segments_intersection_2d(line_start, line_end, start_, end_);
650  }
651 
652  // implements getMinimumDistance() of the base class
653  virtual double getMinimumDistance(const Eigen::Vector2d& position) const
654  {
655  return distance_point_to_segment_2d(position, start_, end_);
656  }
657 
658  // implements getMinimumDistance() of the base class
659  virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
660  {
661  return distance_segment_to_segment_2d(start_, end_, line_start, line_end);
662  }
663 
664  // implements getMinimumDistance() of the base class
665  virtual double getMinimumDistance(const Point2dContainer& polygon) const
666  {
667  return distance_segment_to_polygon_2d(start_, end_, polygon);
668  }
669 
670  // implements getMinimumDistanceVec() of the base class
671  virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const
672  {
673  return closest_point_on_line_segment_2d(position, start_, end_);
674  }
675 
676  // implements getMinimumSpatioTemporalDistance() of the base class
677  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
678  {
679  Eigen::Vector2d offset = t*centroid_velocity_;
680  return distance_point_to_segment_2d(position, start_ + offset, end_ + offset);
681  }
682 
683  // implements getMinimumSpatioTemporalDistance() of the base class
684  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
685  {
686  Eigen::Vector2d offset = t*centroid_velocity_;
687  return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end);
688  }
689 
690  // implements getMinimumSpatioTemporalDistance() of the base class
691  virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
692  {
693  Eigen::Vector2d offset = t*centroid_velocity_;
694  return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon);
695  }
696 
697  // implements getCentroid() of the base class
698  virtual const Eigen::Vector2d& getCentroid() const
699  {
700  return centroid_;
701  }
702 
703  // implements getCentroidCplx() of the base class
704  virtual std::complex<double> getCentroidCplx() const
705  {
706  return std::complex<double>(centroid_.x(), centroid_.y());
707  }
708 
709  // Access or modify line
710  const Eigen::Vector2d& start() const {return start_;}
711  void setStart(const Eigen::Ref<const Eigen::Vector2d>& start) {start_ = start; calcCentroid();}
712  const Eigen::Vector2d& end() const {return end_;}
713  void setEnd(const Eigen::Ref<const Eigen::Vector2d>& end) {end_ = end; calcCentroid();}
714 
715  // implements toPolygonMsg() of the base class
716  virtual void toPolygonMsg(geometry_msgs::Polygon& polygon)
717  {
718  polygon.points.resize(2);
719  polygon.points.front().x = start_.x();
720  polygon.points.front().y = start_.y();
721 
722  polygon.points.back().x = end_.x();
723  polygon.points.back().y = end_.y();
724  polygon.points.back().z = polygon.points.front().z = 0;
725  }
726 
727 protected:
728  void calcCentroid() { centroid_ = 0.5*(start_ + end_); }
729 
730 private:
731  Eigen::Vector2d start_;
732  Eigen::Vector2d end_;
733 
734  Eigen::Vector2d centroid_;
735 
736 public:
737  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
738 };
739 
740 
748 class PolygonObstacle : public Obstacle
749 {
750 public:
751 
755  PolygonObstacle() : Obstacle(), finalized_(false)
756  {
757  centroid_.setConstant(NAN);
758  }
759 
763  PolygonObstacle(const Point2dContainer& vertices) : Obstacle(), vertices_(vertices)
764  {
765  finalizePolygon();
766  }
767 
768 
769  /* FIXME Not working at the moment due to the aligned allocator version of std::vector
770  * And it is C++11 code that is disabled atm to ensure compliance with ROS indigo/jade
771  template <typename... Vector2dType>
772  PolygonObstacle(const Vector2dType&... vertices) : _vertices({vertices...})
773  {
774  calcCentroid();
775  _finalized = true;
776  }
777  */
778 
779 
780  // implements checkCollision() of the base class
781  virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
782  {
783  // line case
784  if (noVertices()==2)
785  return getMinimumDistance(point) <= min_dist;
786 
787  // check if point is in the interior of the polygon
788  // point in polygon test - raycasting (http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html)
789  // using the following algorithm we may obtain false negatives on edge-cases, but that's ok for our purposes
790  int i, j;
791  bool c = false;
792  for (i = 0, j = noVertices()-1; i < noVertices(); j = i++)
793  {
794  if ( ((vertices_.at(i).y()>point.y()) != (vertices_.at(j).y()>point.y())) &&
795  (point.x() < (vertices_.at(j).x()-vertices_.at(i).x()) * (point.y()-vertices_.at(i).y()) / (vertices_.at(j).y()-vertices_.at(i).y()) + vertices_.at(i).x()) )
796  c = !c;
797  }
798  if (c>0) return true;
799 
800  // If this statement is reached, the point lies outside the polygon or maybe on its edges
801  // Let us check the minium distance as well
802  return min_dist == 0 ? false : getMinimumDistance(point) < min_dist;
803  }
804 
805 
814  virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const;
815 
816 
817  // implements getMinimumDistance() of the base class
818  virtual double getMinimumDistance(const Eigen::Vector2d& position) const
819  {
820  return distance_point_to_polygon_2d(position, vertices_);
821  }
822 
823  // implements getMinimumDistance() of the base class
824  virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const
825  {
826  return distance_segment_to_polygon_2d(line_start, line_end, vertices_);
827  }
828 
829  // implements getMinimumDistance() of the base class
830  virtual double getMinimumDistance(const Point2dContainer& polygon) const
831  {
832  return distance_polygon_to_polygon_2d(polygon, vertices_);
833  }
834 
835  // implements getMinimumDistanceVec() of the base class
836  virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const;
837 
838  // implements getMinimumSpatioTemporalDistance() of the base class
839  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const
840  {
841  Point2dContainer pred_vertices;
842  predictVertices(t, pred_vertices);
843  return distance_point_to_polygon_2d(position, pred_vertices);
844  }
845 
846  // implements getMinimumSpatioTemporalDistance() of the base class
847  virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const
848  {
849  Point2dContainer pred_vertices;
850  predictVertices(t, pred_vertices);
851  return distance_segment_to_polygon_2d(line_start, line_end, pred_vertices);
852  }
853 
854  // implements getMinimumSpatioTemporalDistance() of the base class
855  virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const
856  {
857  Point2dContainer pred_vertices;
858  predictVertices(t, pred_vertices);
859  return distance_polygon_to_polygon_2d(polygon, pred_vertices);
860  }
861 
862  virtual void predictVertices(double t, Point2dContainer& pred_vertices) const
863  {
864  // Predict obstacle (polygon) at time t
865  pred_vertices.resize(vertices_.size());
866  Eigen::Vector2d offset = t*centroid_velocity_;
867  for (std::size_t i = 0; i < vertices_.size(); i++)
868  {
869  pred_vertices[i] = vertices_[i] + offset;
870  }
871  }
872 
873  // implements getCentroid() of the base class
874  virtual const Eigen::Vector2d& getCentroid() const
875  {
876  assert(finalized_ && "Finalize the polygon after all vertices are added.");
877  return centroid_;
878  }
879 
880  // implements getCentroidCplx() of the base class
881  virtual std::complex<double> getCentroidCplx() const
882  {
883  assert(finalized_ && "Finalize the polygon after all vertices are added.");
884  return std::complex<double>(centroid_.coeffRef(0), centroid_.coeffRef(1));
885  }
886 
887  // implements toPolygonMsg() of the base class
888  virtual void toPolygonMsg(geometry_msgs::Polygon& polygon);
889 
890 
892 
894  // Access or modify polygon
895  const Point2dContainer& vertices() const {return vertices_;}
896  Point2dContainer& vertices() {return vertices_;}
897 
904  void pushBackVertex(const Eigen::Ref<const Eigen::Vector2d>& vertex)
905  {
906  vertices_.push_back(vertex);
907  finalized_ = false;
908  }
909 
917  void pushBackVertex(double x, double y)
918  {
919  vertices_.push_back(Eigen::Vector2d(x,y));
920  finalized_ = false;
921  }
922 
927  {
928  fixPolygonClosure();
929  calcCentroid();
930  finalized_ = true;
931  }
932 
936  void clearVertices() {vertices_.clear(); finalized_ = false;}
937 
941  int noVertices() const {return (int)vertices_.size();}
942 
943 
945 
946 protected:
947 
948  void fixPolygonClosure();
949 
950  void calcCentroid();
951 
952 
954  Eigen::Vector2d centroid_;
955 
956  bool finalized_;
957 
958 
959 public:
960  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
961 };
962 
963 
964 } // namespace teb_local_planner
965 
966 #endif /* OBSTACLES_H */
virtual void toPolygonMsg(geometry_msgs::Polygon &polygon)=0
Convert the obstacle to a polygon message.
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:532
PointObstacle(const Eigen::Ref< const Eigen::Vector2d > &position)
Construct PointObstacle using a 2d position vector.
Definition: obstacles.h:319
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > VertexContainer
Abbrev. for a container storing vertices (2d points defining the edge points of the polygon) ...
Definition: obstacles.h:601
Eigen::Vector2d pos_
Store the center position of the CircularObstacle.
Definition: obstacles.h:584
Implements a 2D circular obstacle (point obstacle plus radius)
Definition: obstacles.h:447
bool isDynamic() const
Check if the obstacle is a moving with a (non-zero) velocity.
Definition: obstacles.h:199
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.
virtual bool checkCollision(const Eigen::Vector2d &point, double min_dist) const
Check if a given point collides with the obstacle.
Definition: obstacles.h:332
const double & x() const
Return the current y-coordinate of the obstacle (read-only)
Definition: obstacles.h:565
virtual double getMinimumDistance(const Eigen::Vector2d &position) const
Get the minimum euclidean distance to the obstacle (point as reference)
Definition: obstacles.h:818
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &position, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:677
virtual double getMinimumDistance(const Point2dContainer &polygon) const
Get the minimum euclidean distance to the obstacle (polygon as reference)
Definition: obstacles.h:514
void finalizePolygon()
Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods...
Definition: obstacles.h:926
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &position, double t) const =0
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
int noVertices() const
Get the number of vertices defining the polygon (the first vertex is counted once) ...
Definition: obstacles.h:941
void setCentroidVelocity(const geometry_msgs::TwistWithCovariance &velocity, const geometry_msgs::Quaternion &orientation)
Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid.
Definition: obstacles.h:214
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.h:647
const double & y() const
Return the current y-coordinate of the obstacle (read-only)
Definition: obstacles.h:567
CircularObstacle()
Default constructor of the circular obstacle class.
Definition: obstacles.h:454
virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d &position) const =0
Get the closest point on the boundary of the obstacle w.r.t. a specified reference position...
virtual const Eigen::Vector2d & getCentroid() const =0
Get centroid coordinates of the obstacle.
void clearVertices()
Clear all vertices (Afterwards the polygon is not valid anymore)
Definition: obstacles.h:936
virtual ~Obstacle()
Virtual destructor.
Definition: obstacles.h:81
const Point2dContainer & vertices() const
Access vertices container (read-only)
Definition: obstacles.h:895
PolygonObstacle(const Point2dContainer &vertices)
Construct polygon obstacle with a list of vertices.
Definition: obstacles.h:763
PolygonObstacle()
Default constructor of the polygon obstacle class.
Definition: obstacles.h:755
virtual const Eigen::Vector2d & getCentroid() const
Get centroid coordinates of the obstacle.
Definition: obstacles.h:698
PointObstacle()
Default constructor of the point obstacle class.
Definition: obstacles.h:312
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.h:483
CircularObstacle(double x, double y, double radius)
Construct CircularObstacle using x- and y-center-coordinates and radius.
Definition: obstacles.h:471
virtual std::complex< double > getCentroidCplx() const
Get centroid coordinates of the obstacle as complex number.
Definition: obstacles.h:881
double distance_segment_to_segment_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)
Helper function to calculate the smallest distance between two line segments.
virtual double getMinimumSpatioTemporalDistance(const Point2dContainer &polygon, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:691
virtual std::complex< double > getCentroidCplx() const
Get centroid coordinates of the obstacle as complex number.
Definition: obstacles.h:412
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.h:520
boost::shared_ptr< const Obstacle > ObstacleConstPtr
Abbrev. for shared obstacle const pointers.
Definition: obstacles.h:295
PointObstacle(double x, double y)
Construct PointObstacle using x- and y-coordinates.
Definition: obstacles.h:327
const double & x() const
Return the current y-coordinate of the obstacle (read-only)
Definition: obstacles.h:421
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.h:671
virtual const Eigen::Vector2d & getCentroid() const
Get centroid coordinates of the obstacle.
Definition: obstacles.h:874
virtual void toTwistWithCovarianceMsg(geometry_msgs::TwistWithCovariance &twistWithCovariance)
Definition: obstacles.h:264
LineObstacle(const Eigen::Ref< const Eigen::Vector2d > &line_start, const Eigen::Ref< const Eigen::Vector2d > &line_end)
Construct LineObstacle using 2d position vectors as start and end of the line.
Definition: obstacles.h:618
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.h:339
const double & y() const
Return the current y-coordinate of the obstacle (read-only)
Definition: obstacles.h:423
Point2dContainer vertices_
Store vertices defining the polygon (.
Definition: obstacles.h:953
virtual void predictCentroidConstantVelocity(double t, Eigen::Ref< Eigen::Vector2d > position) const
Predict position of the centroid assuming a constant velocity model.
Definition: obstacles.h:544
Eigen::Vector2d & position()
Return the current position of the obstacle.
Definition: obstacles.h:563
virtual double getMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const
Get the minimum euclidean distance to the obstacle (line as reference)
Definition: obstacles.h:508
LineObstacle()
Default constructor of the point obstacle class.
Definition: obstacles.h:606
void setCentroidVelocity(const Eigen::Ref< const Eigen::Vector2d > &vel)
Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid.
Definition: obstacles.h:206
std::vector< Eigen::Vector2d, Eigen::aligned_allocator< Eigen::Vector2d > > Point2dContainer
Abbrev. for a container storing 2d points.
virtual double getMinimumDistance(const Point2dContainer &polygon) const
Get the minimum euclidean distance to the obstacle (polygon as reference)
Definition: obstacles.h:665
virtual void toPolygonMsg(geometry_msgs::Polygon &polygon)
Convert the obstacle to a polygon message.
Definition: obstacles.h:572
virtual double getMinimumSpatioTemporalDistance(const Point2dContainer &polygon, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:394
virtual std::complex< double > getCentroidCplx() const
Get centroid coordinates of the obstacle as complex number.
Definition: obstacles.h:704
virtual void toPolygonMsg(geometry_msgs::Polygon &polygon)
Convert the obstacle to a polygon message.
Definition: obstacles.h:716
Implements a polygon obstacle with an arbitrary number of vertices.
Definition: obstacles.h:748
Implements a 2D line obstacle.
Definition: obstacles.h:597
virtual void predictVertices(double t, Point2dContainer &pred_vertices) const
Definition: obstacles.h:862
Obstacle()
Default constructor of the abstract obstacle class.
Definition: obstacles.h:74
virtual double getMinimumDistance(const Eigen::Vector2d &position) const
Get the minimum euclidean distance to the obstacle (point as reference)
Definition: obstacles.h:653
boost::shared_ptr< Obstacle > ObstaclePtr
Abbrev. for shared obstacle pointers.
Definition: obstacles.h:293
TFSIMD_FORCE_INLINE Vector3 normalized() const
const Eigen::Vector2d & position() const
Return the current position of the obstacle (read-only)
Definition: obstacles.h:418
Point2dContainer & vertices()
Access vertices container.
Definition: obstacles.h:896
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 double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:684
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &position, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:839
const double & radius() const
Return the current radius of the obstacle.
Definition: obstacles.h:569
virtual double getMinimumDistance(const Eigen::Vector2d &position) const
Get the minimum euclidean distance to the obstacle (point as reference)
Definition: obstacles.h:358
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.h:376
bool finalized_
Flat that keeps track if the polygon was finalized after adding all vertices.
Definition: obstacles.h:956
virtual void predictCentroidConstantVelocity(double t, Eigen::Ref< Eigen::Vector2d > position) const
Predict position of the centroid assuming a constant velocity model.
Definition: obstacles.h:400
virtual std::complex< double > getCentroidCplx() const =0
Get centroid coordinates of the obstacle as complex number.
virtual double getMinimumSpatioTemporalDistance(const Point2dContainer &polygon, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:855
double & x()
Return the current x-coordinate of the obstacle.
Definition: obstacles.h:420
virtual void toPolygonMsg(geometry_msgs::Polygon &polygon)
Convert the obstacle to a polygon message.
Definition: obstacles.h:426
Implements a 2D point obstacle.
Definition: obstacles.h:305
std::vector< ObstaclePtr > ObstContainer
Abbrev. for containers storing multiple obstacles.
Definition: obstacles.h:297
virtual double getMinimumDistance(const Eigen::Vector2d &position) const
Get the minimum euclidean distance to the obstacle (point as reference)
Definition: obstacles.h:502
virtual bool checkCollision(const Eigen::Vector2d &position, double min_dist) const =0
Check if a given point collides with the obstacle.
virtual double getMinimumSpatioTemporalDistance(const Point2dContainer &polygon, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:538
void pushBackVertex(const Eigen::Ref< const Eigen::Vector2d > &vertex)
Add a vertex to the polygon (edge-point)
Definition: obstacles.h:904
virtual double getMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const
Get the minimum euclidean distance to the obstacle (line as reference)
Definition: obstacles.h:364
virtual double getMinimumDistance(const Eigen::Vector2d &position) const =0
Get the minimum euclidean distance to the obstacle (point as reference)
double & y()
Return the current x-coordinate of the obstacle.
Definition: obstacles.h:566
double distance_point_to_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 calculate the distance between a line segment and a point.
const Eigen::Vector2d & end() const
Definition: obstacles.h:712
void pushBackVertex(double x, double y)
Add a vertex to the polygon (edge-point)
Definition: obstacles.h:917
double distance_polygon_to_polygon_2d(const Point2dContainer &vertices1, const Point2dContainer &vertices2)
Helper function to calculate the smallest distance between two closed polygons.
double distance_point_to_polygon_2d(const Eigen::Vector2d &point, const Point2dContainer &vertices)
Helper function to calculate the smallest distance between a point and a closed polygon.
Eigen::Vector2d centroid_velocity_
Store the corresponding velocity (vx, vy) of the centroid (zero, if _dynamic is true) ...
Definition: obstacles.h:285
void setStart(const Eigen::Ref< const Eigen::Vector2d > &start)
Definition: obstacles.h:711
void setCentroidVelocity(const geometry_msgs::TwistWithCovariance &velocity, const geometry_msgs::QuaternionStamped &orientation)
Definition: obstacles.h:235
CircularObstacle(const Eigen::Ref< const Eigen::Vector2d > &position, double radius)
Construct CircularObstacle using a 2d center position vector and radius.
Definition: obstacles.h:462
virtual double getMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const
Get the minimum euclidean distance to the obstacle (line as reference)
Definition: obstacles.h:659
const Eigen::Vector2d & start() const
Definition: obstacles.h:710
Eigen::Vector2d & position()
Return the current position of the obstacle.
Definition: obstacles.h:419
const Eigen::Vector2d & getCentroidVelocity() const
Get the obstacle velocity (vx, vy) (w.r.t. to the centroid)
Definition: obstacles.h:245
virtual std::complex< double > getCentroidCplx() const
Get centroid coordinates of the obstacle as complex number.
Definition: obstacles.h:556
virtual const Eigen::Vector2d & getCentroid() const
Get centroid coordinates of the obstacle.
Definition: obstacles.h:406
virtual double getMinimumDistance(const Point2dContainer &polygon) const
Get the minimum euclidean distance to the obstacle (polygon as reference)
Definition: obstacles.h:370
double distance_segment_to_polygon_2d(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, const Point2dContainer &vertices)
Helper function to calculate the smallest distance between a line segment and a closed polygon...
virtual double getMinimumDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end) const
Get the minimum euclidean distance to the obstacle (line as reference)
Definition: obstacles.h:824
Eigen::Vector2d centroid_
Store the centroid coordinates of the polygon (.
Definition: obstacles.h:954
double & radius()
Return the current radius of the obstacle.
Definition: obstacles.h:568
virtual bool checkCollision(const Eigen::Vector2d &point, double min_dist) const
Check if a given point collides with the obstacle.
Definition: obstacles.h:476
Eigen::Vector2d pos_
Store the position of the PointObstacle.
Definition: obstacles.h:436
const Eigen::Vector2d & position() const
Return the current position of the obstacle (read-only)
Definition: obstacles.h:562
virtual bool checkCollision(const Eigen::Vector2d &point, double min_dist) const
Check if a given point collides with the obstacle.
Definition: obstacles.h:781
virtual bool checkLineIntersection(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double min_dist=0) const =0
Check if a given line segment between two points intersects with the obstacle (and additionally keeps...
bool dynamic_
Store flag if obstacle is dynamic (resp. a moving obstacle)
Definition: obstacles.h:284
virtual double getMinimumDistance(const Point2dContainer &polygon) const
Get the minimum euclidean distance to the obstacle (polygon as reference)
Definition: obstacles.h:830
virtual void predictCentroidConstantVelocity(double t, Eigen::Ref< Eigen::Vector2d > position) const
Predict position of the centroid assuming a constant velocity model.
Definition: obstacles.h:190
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &position, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:382
virtual const Eigen::Vector2d & getCentroid() const
Get centroid coordinates of the obstacle.
Definition: obstacles.h:550
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:847
LineObstacle(double x1, double y1, double x2, double y2)
Construct LineObstacle using start and end coordinates.
Definition: obstacles.h:631
virtual bool checkCollision(const Eigen::Vector2d &point, double min_dist) const
Check if a given point collides with the obstacle.
Definition: obstacles.h:641
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &position, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:526
virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d &line_start, const Eigen::Vector2d &line_end, double t) const
Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity mo...
Definition: obstacles.h:388
double & x()
Return the current x-coordinate of the obstacle.
Definition: obstacles.h:564
double & y()
Return the current x-coordinate of the obstacle.
Definition: obstacles.h:422
Abstract class that defines the interface for modelling obstacles.
Definition: obstacles.h:67
void setEnd(const Eigen::Ref< const Eigen::Vector2d > &end)
Definition: obstacles.h:713


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