vertex_timediff.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 VERTEX_TIMEDIFF_H
45 #define VERTEX_TIMEDIFF_H
46 
47 
48 #include "g2o/config.h"
49 #include "g2o/core/base_vertex.h"
50 #include "g2o/core/hyper_graph_action.h"
51 
52 #include "ros/console.h"
53 
54 #include <Eigen/Core>
55 
56 namespace teb_local_planner
57 {
58 
65 class VertexTimeDiff : public g2o::BaseVertex<1, double>
66 {
67 public:
68 
73  VertexTimeDiff(bool fixed = false)
74  {
76  setFixed(fixed);
77  }
78 
84  VertexTimeDiff(double dt, bool fixed = false)
85  {
86  _estimate = dt;
87  setFixed(fixed);
88  }
89 
94  {}
95 
101  double& dt() {return _estimate;}
102 
108  const double& dt() const {return _estimate;}
109 
113  virtual void setToOriginImpl()
114  {
115  _estimate = 0.1;
116  }
117 
123  virtual void oplusImpl(const double* update)
124  {
125  _estimate += *update;
126  }
127 
133  virtual bool read(std::istream& is)
134  {
135  is >> _estimate;
136  return true;
137  }
138 
144  virtual bool write(std::ostream& os) const
145  {
146  os << estimate();
147  return os.good();
148  }
149 
150  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
151 };
152 
153 }
154 
155 #endif
virtual bool write(std::ostream &os) const
Write the estimate to an output stream.
virtual bool read(std::istream &is)
Read an estimate of from an input stream.
VertexTimeDiff(double dt, bool fixed=false)
Construct the TimeDiff vertex with a value.
const double & dt() const
Access the timediff value of the vertex (read-only)
~VertexTimeDiff()
Destructs the VertexTimeDiff.
virtual void oplusImpl(const double *update)
Define the update increment . A simple addition implements what we want.
VertexTimeDiff(bool fixed=false)
Default constructor.
virtual void setToOriginImpl()
Set the underlying TimeDiff estimate to default.
double & dt()
Access the timediff value of the vertex.
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