orientation_filter.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, David V. Lu!!
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of David V. Lu nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: David V. Lu!!
36  *********************************************************************/
38 #include <tf/tf.h>
39 #include <angles/angles.h>
40 
41 namespace global_planner {
42 
43 void set_angle(geometry_msgs::PoseStamped* pose, double angle)
44 {
45  pose->pose.orientation = tf::createQuaternionMsgFromYaw(angle);
46 }
47 
48 double getYaw(geometry_msgs::PoseStamped pose)
49 {
50  return tf::getYaw(pose.pose.orientation);
51 }
52 
53 void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
54  std::vector<geometry_msgs::PoseStamped>& path)
55 {
56  int n = path.size();
57  if (n == 0) return;
58  switch(omode_) {
59  case FORWARD:
60  for(int i=0;i<n-1;i++){
62  }
63  break;
64  case BACKWARD:
65  for(int i=0;i<n-1;i++){
67  set_angle(&path[i], angles::normalize_angle(getYaw(path[i]) + M_PI));
68  }
69  break;
70  case LEFTWARD:
71  for(int i=0;i<n-1;i++){
73  set_angle(&path[i], angles::normalize_angle(getYaw(path[i]) - M_PI_2));
74  }
75  break;
76  case RIGHTWARD:
77  for(int i=0;i<n-1;i++){
79  set_angle(&path[i], angles::normalize_angle(getYaw(path[i]) + M_PI_2));
80  }
81  break;
82  case INTERPOLATE:
83  path[0].pose.orientation = start.pose.orientation;
84  interpolate(path, 0, n-1);
85  break;
87  for(int i=0;i<n-1;i++){
89  }
90 
91  int i=n-3;
92  double last = getYaw(path[i]);
93  while( i>0 ){
94  double new_angle = getYaw(path[i-1]);
95  double diff = fabs(angles::shortest_angular_distance(new_angle, last));
96  if( diff>0.35)
97  break;
98  else
99  i--;
100  }
101 
102  path[0].pose.orientation = start.pose.orientation;
103  interpolate(path, i, n-1);
104  break;
105  }
106 }
107 
108 void OrientationFilter::setAngleBasedOnPositionDerivative(std::vector<geometry_msgs::PoseStamped>& path, int index)
109 {
110  int index0 = std::max(0, index - window_size_);
111  int index1 = std::min((int)path.size() - 1, index + window_size_);
112 
113  double x0 = path[index0].pose.position.x,
114  y0 = path[index0].pose.position.y,
115  x1 = path[index1].pose.position.x,
116  y1 = path[index1].pose.position.y;
117 
118  double angle = atan2(y1-y0,x1-x0);
119  set_angle(&path[index], angle);
120 }
121 
122 void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& path,
123  int start_index, int end_index)
124 {
125  double start_yaw = getYaw(path[start_index]),
126  end_yaw = getYaw(path[end_index ]);
127  double diff = angles::shortest_angular_distance(start_yaw, end_yaw);
128  double increment = diff/(end_index-start_index);
129  for(int i=start_index; i<=end_index; i++){
130  double angle = start_yaw + increment * i;
131  set_angle(&path[i], angle);
132  }
133 }
134 
135 
136 };
virtual void processPath(const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > &path)
static double getYaw(const Quaternion &bt_q)
void set_angle(geometry_msgs::PoseStamped *pose, double angle)
void interpolate(std::vector< geometry_msgs::PoseStamped > &path, int start_index, int end_index)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void setAngleBasedOnPositionDerivative(std::vector< geometry_msgs::PoseStamped > &path, int index)
def normalize_angle(angle)
double getYaw(geometry_msgs::PoseStamped pose)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
def shortest_angular_distance(from_angle, to_angle)


global_planner
Author(s): David Lu!!
autogenerated on Thu Mar 12 2020 04:10:23