edge_prefer_rotdir.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_PREFER_ROTDIR_H_
44 #define EDGE_PREFER_ROTDIR_H_
45 
49 #include "g2o/core/base_unary_edge.h"
50 
51 
52 namespace teb_local_planner
53 {
54 
65 class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose>
66 {
67 public:
68 
73  {
74  _measurement = 1;
75  }
76 
80  void computeError()
81  {
82  const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]);
83  const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]);
84 
85  _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0);
86 
87  ROS_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]);
88  }
89 
94  void setRotDir(double dir)
95  {
96  _measurement = dir;
97  }
98 
100  void preferRight() {_measurement = -1;}
101 
103  void preferLeft() {_measurement = 1;}
104 
105 
106 public:
107  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
108 
109 };
110 
111 
112 
113 } // end namespace
114 
115 #endif
double penaltyBoundFromBelow(const double &var, const double &a, const double &epsilon)
Linear penalty function for bounding var from below: .
Definition: penalties.h:107
Edge defining the cost function for penalzing a specified turning direction, in particular left resp...
void setRotDir(double dir)
Specify the prefered direction of rotation.
void computeError()
Actual cost function.
double & theta()
Access the orientation part (yaw angle) of the pose.
Definition: vertex_pose.h:178
#define ROS_ASSERT_MSG(cond,...)
Base edge connecting two vertices in the TEB optimization problem.
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
Definition: vertex_pose.h:63


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