edge_time_optimal.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
42  *********************************************************************/
43 
44 #ifndef EDGE_TIMEOPTIMAL_H_
45 #define EDGE_TIMEOPTIMAL_H_
46 
47 #include <float.h>
48 
49 #include <base_local_planner/BaseLocalPlannerConfig.h>
50 
55 
56 #include <Eigen/Core>
57 
58 namespace teb_local_planner
59 {
60 
61 
73 class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff>
74 {
75 public:
76 
81  {
82  this->setMeasurement(0.);
83  }
84 
88  void computeError()
89  {
90  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
91  const VertexTimeDiff* timediff = static_cast<const VertexTimeDiff*>(_vertices[0]);
92 
93  _error[0] = timediff->dt();
94 
95  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]);
96  }
97 
98 #ifdef USE_ANALYTIC_JACOBI
99 
102  void linearizeOplus()
103  {
104  ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()");
105  _jacobianOplusXi( 0 , 0 ) = 1;
106  }
107 #endif
108 
109 
110 public:
111  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
112 };
113 
114 }; // end namespace
115 
116 #endif /* EDGE_TIMEOPTIMAL_H_ */
Edge defining the cost function for minimizing transition time of the trajectory. ...
const TebConfig * cfg_
Store TebConfig class for parameters.
void computeError()
Actual cost function.
#define ROS_ASSERT_MSG(cond,...)
double & dt()
Access the timediff value of the vertex.
Base edge connecting a single vertex in the TEB optimization problem.
This class stores and wraps a time difference into a vertex that can be optimized via g2o...


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