[global_planner] Add a mode automatically deciding Forward or Backward

smilerobotics · Jun 19, 2021 · 621fd87 · 621fd87
1 parent 2b807bd
commit 621fd87
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 6 deletions.
diff --git a/global_planner/cfg/GlobalPlanner.cfg b/global_planner/cfg/GlobalPlanner.cfg
@@ -23,9 +23,11 @@ orientation_enum = gen.enum([
               "Positive y axis points along the path, except for the goal orientation"),
     gen.const("Rightward", int_t, 6,
               "Negative y axis points along the path, except for the goal orientation"),
+    gen.const("Automatic", int_t, 7,
+              "Automatically use either Forward (1) or Backward (4)"),
 ], "How to set the orientation of each point")
 
-gen.add("orientation_mode", int_t, 0, "How to set the orientation of each point", 1, 0, 6,
+gen.add("orientation_mode", int_t, 0, "How to set the orientation of each point", 1, 0, 7,
         edit_method=orientation_enum)
 gen.add("orientation_window_size", int_t, 0, "What window to use to determine the orientation based on the "
         "position derivative specified by the orientation mode", 1, 1, 255)

diff --git a/global_planner/include/global_planner/orientation_filter.h b/global_planner/include/global_planner/orientation_filter.h
@@ -40,7 +40,7 @@
 
 namespace global_planner {
 
-enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD };
+enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD, AUTOMATIC };
 
 class OrientationFilter {
     public:

diff --git a/global_planner/src/orientation_filter.cpp b/global_planner/src/orientation_filter.cpp
@@ -83,6 +83,7 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
             interpolate(path, 0, n-1);
             break;
         case FORWARDTHENINTERPOLATE:
+        {
             for(int i=0;i<n-1;i++){
                 setAngleBasedOnPositionDerivative(path, i);
             }
@@ -100,10 +101,37 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
 
             path[0].pose.orientation = start.pose.orientation;
             interpolate(path, i, n-1);
-            break;           
+            break;
+        }
+        case AUTOMATIC:
+            for(int i = 0; i < n - 1; i++) {
+                setAngleBasedOnPositionDerivative(path, i);
+            }
+            if (n < 2) {
+                return;
+            }
+
+            const double forward_start_rotation =
+                std::abs(tf2::getYaw(path[0].pose.orientation) - tf2::getYaw(start.pose.orientation));
+            const double forward_goal_rotation =
+                std::abs(tf2::getYaw(path[n - 2].pose.orientation) - tf2::getYaw(path[n - 1].pose.orientation));
+
+            const double forward_additional_rotation = forward_start_rotation + forward_goal_rotation;
+            const double backward_additional_rotation
+            = std::abs(forward_additional_rotation - 2.0 * M_PI);
+
+            // check whether FORWARD or BACKWARD is optimal in rotation
+            // * nothing to do for using FORWARD
+            if (forward_additional_rotation > backward_additional_rotation) {
+                // use BACKWARD
+                for(int i = 0; i < n - 1; i++) {
+                    set_angle(&path[i], angles::normalize_angle(tf2::getYaw(path[i].pose.orientation) + M_PI));
+                }
+            }
+            break;
     }
 }
-    
+
 void OrientationFilter::setAngleBasedOnPositionDerivative(std::vector<geometry_msgs::PoseStamped>& path, int index)
 {
   int index0 = std::max(0, index - window_size_);
@@ -130,6 +158,4 @@ void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& pat
         set_angle(&path[i], angle);
     }
 }
-
-
 };