Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <global_planner/orientation_filter.h>
00038 #include <tf/tf.h>
00039 #include <angles/angles.h>
00040
00041 namespace global_planner {
00042
00043 void set_angle(geometry_msgs::PoseStamped* pose, double angle)
00044 {
00045 pose->pose.orientation = tf::createQuaternionMsgFromYaw(angle);
00046 }
00047
00048 double getYaw(geometry_msgs::PoseStamped pose)
00049 {
00050 return tf::getYaw(pose.pose.orientation);
00051 }
00052
00053 void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
00054 std::vector<geometry_msgs::PoseStamped>& path)
00055 {
00056 int n = path.size();
00057 if (n == 0) return;
00058 switch(omode_) {
00059 case FORWARD:
00060 for(int i=0;i<n-1;i++){
00061 pointToNext(path, i);
00062 }
00063 break;
00064 case INTERPOLATE:
00065 path[0].pose.orientation = start.pose.orientation;
00066 interpolate(path, 0, n-1);
00067 break;
00068 case FORWARDTHENINTERPOLATE:
00069 for(int i=0;i<n-1;i++){
00070 pointToNext(path, i);
00071 }
00072
00073 int i=n-3;
00074 double last = getYaw(path[i]);
00075 while( i>0 ){
00076 double new_angle = getYaw(path[i-1]);
00077 double diff = fabs(angles::shortest_angular_distance(new_angle, last));
00078 if( diff>0.35)
00079 break;
00080 else
00081 i--;
00082 }
00083
00084 path[0].pose.orientation = start.pose.orientation;
00085 interpolate(path, i, n-1);
00086 break;
00087 }
00088 }
00089
00090 void OrientationFilter::pointToNext(std::vector<geometry_msgs::PoseStamped>& path, int index)
00091 {
00092 double x0 = path[ index ].pose.position.x,
00093 y0 = path[ index ].pose.position.y,
00094 x1 = path[index+1].pose.position.x,
00095 y1 = path[index+1].pose.position.y;
00096
00097 double angle = atan2(y1-y0,x1-x0);
00098 set_angle(&path[index], angle);
00099 }
00100
00101 void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& path,
00102 int start_index, int end_index)
00103 {
00104 double start_yaw = getYaw(path[start_index]),
00105 end_yaw = getYaw(path[end_index ]);
00106 double diff = angles::shortest_angular_distance(start_yaw, end_yaw);
00107 double increment = diff/(end_index-start_index);
00108 for(int i=start_index; i<=end_index; i++){
00109 double angle = start_yaw + increment * i;
00110 set_angle(&path[i], angle);
00111 }
00112 }
00113
00114
00115 };