edge_via_point.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 #ifndef EDGE_VIA_POINT_H_
44 #define EDGE_VIA_POINT_H_
45 
48 
49 #include "g2o/core/base_unary_edge.h"
50 
51 
52 namespace teb_local_planner
53 {
54 
66 class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose>
67 {
68 public:
69 
74  {
75  _measurement = NULL;
76  }
77 
81  void computeError()
82  {
83  ROS_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()");
84  const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]);
85 
86  _error[0] = (bandpt->position() - *_measurement).norm();
87 
88  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]);
89  }
90 
95  void setViaPoint(const Eigen::Vector2d* via_point)
96  {
97  _measurement = via_point;
98  }
99 
105  void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point)
106  {
107  cfg_ = &cfg;
108  _measurement = via_point;
109  }
110 
111 public:
112  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
113 
114 };
115 
116 
117 
118 } // end namespace
119 
120 #endif
Config class for the teb_local_planner and its components.
Definition: teb_config.h:61
const TebConfig * cfg_
Store TebConfig class for parameters.
void computeError()
Actual cost function.
#define ROS_ASSERT_MSG(cond,...)
Eigen::Vector2d & position()
Access the 2D position part.
Definition: vertex_pose.h:141
void setParameters(const TebConfig &cfg, const Eigen::Vector2d *via_point)
Set all parameters at once.
void setViaPoint(const Eigen::Vector2d *via_point)
Set pointer to associated via point for the underlying cost function.
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
Definition: vertex_pose.h:63
Edge defining the cost function for pushing a configuration towards a via point.
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