Add FixedOrientation (Omni) and Sideward orientation modes (#89)

rapyuta-robotics · Nov 8, 2023 · 6239527 · 6239527
1 parent 904f16e
commit 6239527
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 3 deletions.
diff --git a/global_planner/cfg/GlobalPlanner.cfg b/global_planner/cfg/GlobalPlanner.cfg
@@ -26,9 +26,13 @@ orientation_enum = gen.enum([
               "Negative y axis points along the path, except for the goal orientation"),
     gen.const("Bidirectional", int_t, 7,
               "Forward or backward depending on what requires less turning from start to path and from path to goal"),
+    gen.const("FixedOrientation", int_t, 8,
+              "Keep the same orientation as the orientation at the start pose, except at the goal"),
+    gen.const("Sideward", int_t, 9,
+              "Positive or negative y axis points along the path depending on what requires less turning from start to path and from path to goal"),
 ], "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, 7,
+gen.add("orientation_mode", int_t, 0, "How to set the orientation of each point", 1, 0, 9,
         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, BIDIRECTIONAL };
+enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD, BIDIRECTIONAL, FIXEDORIENTATION, SIDEWARD };
 
 class OrientationFilter {
     public:

diff --git a/global_planner/src/orientation_filter.cpp b/global_planner/src/orientation_filter.cpp
@@ -97,6 +97,26 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
                 }
             }
             break;
+        case SIDEWARD:
+            for (int i = 0; i < n - 1; i++) {
+                setAngleBasedOnPositionDerivative(path, i);
+            }
+            if (n > 2) {
+                const int num_skips = n >= 5 ? 2 : 1;
+                const double start_to_path_theta =  min_angle(start, *std::next(path.begin(), num_skips));
+                const double path_to_goal_theta =  min_angle(*std::prev(path.end(), 1 + num_skips), path.back());
+
+                const bool prefer_leftward = 
+                    std::fabs(angles::normalize_angle(start_to_path_theta - M_PI_2)) + std::fabs(angles::normalize_angle(path_to_goal_theta + M_PI_2)) < 
+                    std::fabs(angles::normalize_angle(start_to_path_theta + M_PI_2)) + std::fabs(angles::normalize_angle(path_to_goal_theta - M_PI_2));
+
+                for (int i = 0; i < n - 1; i++) {
+                    const double path_orientation = tf2::getYaw(path[i].pose.orientation);
+                    const double adjusted_orientation = prefer_leftward ? path_orientation - M_PI_2 : path_orientation + M_PI_2;
+                    set_angle(&path[i], angles::normalize_angle(adjusted_orientation));
+                }
+            }
+            break;
         case LEFTWARD:
             for(int i=0;i<n-1;i++){
                 setAngleBasedOnPositionDerivative(path, i);
@@ -109,6 +129,11 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
                 set_angle(&path[i], angles::normalize_angle(tf2::getYaw(path[i].pose.orientation) + M_PI_2));
             }
             break;
+        case FIXEDORIENTATION:
+            for(int i=0;i<n-1;i++){
+                path[i].pose.orientation = start.pose.orientation;
+            }
+            break;
         case INTERPOLATE:
             path[0].pose.orientation = start.pose.orientation;
             interpolate(path, 0, n-1);
@@ -161,6 +186,6 @@ void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& pat
         set_angle(&path[i], angle);
     }
 }
-                                   
+
 
 };