base_teb_edges.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 _BASE_TEB_EDGES_H_
45 #define _BASE_TEB_EDGES_H_
46 
48 
49 #include <g2o/core/base_binary_edge.h>
50 #include <g2o/core/base_unary_edge.h>
51 #include <g2o/core/base_multi_edge.h>
52 
53 #include <cmath>
54 
55 namespace teb_local_planner
56 {
57 
58 
69 template <int D, typename E, typename VertexXi>
70 class BaseTebUnaryEdge : public g2o::BaseUnaryEdge<D, E, VertexXi>
71 {
72 public:
73 
74  using typename g2o::BaseUnaryEdge<D, E, VertexXi>::ErrorVector;
75  using g2o::BaseUnaryEdge<D, E, VertexXi>::computeError;
76 
81  {
82  _vertices[0] = NULL;
83  }
84 
92  {
93  if(_vertices[0])
94  _vertices[0]->edges().erase(this);
95  }
96 
103  ErrorVector& getError()
104  {
105  computeError();
106  return _error;
107  }
108 
112  virtual bool read(std::istream& is)
113  {
114  // TODO generic read
115  return true;
116  }
117 
121  virtual bool write(std::ostream& os) const
122  {
123  // TODO generic write
124  return os.good();
125  }
126 
131  void setTebConfig(const TebConfig& cfg)
132  {
133  cfg_ = &cfg;
134  }
135 
136 protected:
137 
138  using g2o::BaseUnaryEdge<D, E, VertexXi>::_error;
139  using g2o::BaseUnaryEdge<D, E, VertexXi>::_vertices;
140 
141  const TebConfig* cfg_;
142 
143 public:
144  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145 };
146 
157 template <int D, typename E, typename VertexXi, typename VertexXj>
158 class BaseTebBinaryEdge : public g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>
159 {
160 public:
161 
162  using typename g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::ErrorVector;
163  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::computeError;
164 
169  {
170  _vertices[0] = _vertices[1] = NULL;
171  }
172 
180  {
181  if(_vertices[0])
182  _vertices[0]->edges().erase(this);
183  if(_vertices[1])
184  _vertices[1]->edges().erase(this);
185  }
186 
193  ErrorVector& getError()
194  {
195  computeError();
196  return _error;
197  }
198 
202  virtual bool read(std::istream& is)
203  {
204  // TODO generic read
205  return true;
206  }
207 
211  virtual bool write(std::ostream& os) const
212  {
213  // TODO generic write
214  return os.good();
215  }
216 
221  void setTebConfig(const TebConfig& cfg)
222  {
223  cfg_ = &cfg;
224  }
225 
226 protected:
227 
228  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_error;
229  using g2o::BaseBinaryEdge<D, E, VertexXi, VertexXj>::_vertices;
230 
231  const TebConfig* cfg_;
232 
233 public:
234  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
235 };
236 
237 
248 template <int D, typename E>
249 class BaseTebMultiEdge : public g2o::BaseMultiEdge<D, E>
250 {
251 public:
252 
253  using typename g2o::BaseMultiEdge<D, E>::ErrorVector;
254  using g2o::BaseMultiEdge<D, E>::computeError;
255 
260  {
261 // for(std::size_t i=0; i<_vertices.size(); ++i)
262 // _vertices[i] = NULL;
263  }
264 
272  {
273  for(std::size_t i=0; i<_vertices.size(); ++i)
274  {
275  if(_vertices[i])
276  _vertices[i]->edges().erase(this);
277  }
278  }
279 
280  // Overwrites resize() from the parent class
281  virtual void resize(size_t size)
282  {
283  g2o::BaseMultiEdge<D, E>::resize(size);
284 
285  for(std::size_t i=0; i<_vertices.size(); ++i)
286  _vertices[i] = NULL;
287  }
288 
295  ErrorVector& getError()
296  {
297  computeError();
298  return _error;
299  }
300 
304  virtual bool read(std::istream& is)
305  {
306  // TODO generic read
307  return true;
308  }
309 
313  virtual bool write(std::ostream& os) const
314  {
315  // TODO generic write
316  return os.good();
317  }
318 
323  void setTebConfig(const TebConfig& cfg)
324  {
325  cfg_ = &cfg;
326  }
327 
328 protected:
329 
330  using g2o::BaseMultiEdge<D, E>::_error;
331  using g2o::BaseMultiEdge<D, E>::_vertices;
332 
333  const TebConfig* cfg_;
334 
335 public:
336  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
337 };
338 
339 
340 
341 
342 
343 
344 } // end namespace
345 
346 #endif
virtual void resize(size_t size)
virtual bool write(std::ostream &os) const
Write values to an output stream.
virtual bool write(std::ostream &os) const
Write values to an output stream.
void setTebConfig(const TebConfig &cfg)
Assign the TebConfig class for parameters.
virtual ~BaseTebMultiEdge()
Destruct edge.
ErrorVector & getError()
Compute and return error / cost value.
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
virtual bool read(std::istream &is)
Read values from input stream.
void setTebConfig(const TebConfig &cfg)
Assign the TebConfig class for parameters.
const TebConfig * cfg_
Store TebConfig class for parameters.
virtual bool write(std::ostream &os) const
Write values to an output stream.
virtual bool read(std::istream &is)
Read values from input stream.
void setTebConfig(const TebConfig &cfg)
Assign the TebConfig class for parameters.
Base edge connecting two vertices in the TEB optimization problem.
ErrorVector & getError()
Compute and return error / cost value.
const TebConfig * cfg_
Store TebConfig class for parameters.
ErrorVector & getError()
Compute and return error / cost value.
Base edge connecting two vertices in the TEB optimization problem.
virtual ~BaseTebBinaryEdge()
Destruct edge.
const TebConfig * cfg_
Store TebConfig class for parameters.
virtual ~BaseTebUnaryEdge()
Destruct edge.
virtual bool read(std::istream &is)
Read values from input stream.
Base edge connecting a single vertex in the TEB optimization problem.


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