edge_dynamic_obstacle.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  * Notes:
37  * The following class is derived from a class defined by the
38  * g2o-framework. g2o is licensed under the terms of the BSD License.
39  * Refer to the base class source for detailed licensing information.
40  *
41  * Author: Christoph Rösmann, Franz Albers
42  *********************************************************************/
43 
44 #ifndef EDGE_DYNAMICOBSTACLE_H
45 #define EDGE_DYNAMICOBSTACLE_H
46 
54 
55 namespace teb_local_planner
56 {
57 
71 class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose>
72 {
73 public:
74 
79  {
80  }
81 
86  EdgeDynamicObstacle(double t) : t_(t)
87  {
88  }
89 
93  void computeError()
94  {
95  ROS_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()");
96  const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
97 
98  double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_);
99 
102 
103  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]);
104  }
105 
106 
111  void setObstacle(const Obstacle* obstacle)
112  {
113  _measurement = obstacle;
114  }
115 
120  void setRobotModel(const BaseRobotFootprintModel* robot_model)
121  {
122  robot_model_ = robot_model;
123  }
124 
131  void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle)
132  {
133  cfg_ = &cfg;
134  robot_model_ = robot_model;
135  _measurement = obstacle;
136  }
137 
138 protected:
139 
141  double t_;
142 
143 public:
144  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145 
146 };
147 
148 
149 
150 
151 } // end namespace
152 
153 #endif
double penaltyBoundFromBelow(const double &var, const double &a, const double &epsilon)
Linear penalty function for bounding var from below: .
Definition: penalties.h:107
PoseSE2 & pose()
Access the pose.
Definition: vertex_pose.h:126
Edge defining the cost function for keeping a distance from dynamic (moving) obstacles.
double min_obstacle_dist
Minimum desired separation from obstacles.
Definition: teb_config.h:119
void setRobotModel(const BaseRobotFootprintModel *robot_model)
Set pointer to the robot model.
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
Abstract class that defines the interface for robot footprint/contour models.
EdgeDynamicObstacle(double t)
Construct edge and specify the time for its associated pose (neccessary for computeError).
const TebConfig * cfg_
Store TebConfig class for parameters.
void computeError()
Actual cost function.
double t_
Estimated time until current pose is reached.
virtual double estimateSpatioTemporalDistance(const PoseSE2 &current_pose, const Obstacle *obstacle, double t) const =0
Estimate the distance between the robot and the predicted location of an obstacle at time t...
const BaseRobotFootprintModel * robot_model_
Store pointer to robot_model.
#define ROS_ASSERT_MSG(cond,...)
double penalty_epsilon
Add a small safety margin to penalty functions for hard-constraint approximations.
Definition: teb_config.h:144
void setObstacle(const Obstacle *obstacle)
Set Obstacle for the underlying cost function.
void setParameters(const TebConfig &cfg, const BaseRobotFootprintModel *robot_model, const Obstacle *obstacle)
Set all parameters at once.
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
Definition: vertex_pose.h:63
double dynamic_obstacle_inflation_dist
Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be la...
Definition: teb_config.h:121
struct teb_local_planner::TebConfig::Optimization optim
Optimization related parameters.
Base edge connecting a single vertex in the TEB optimization problem.
struct teb_local_planner::TebConfig::Obstacles obstacles
Obstacle related parameters.
Abstract class that defines the interface for modelling obstacles.
Definition: obstacles.h:67


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 3 2020 04:03:08