fix angle

rapyuta-robotics · Nov 6, 2023 · 650b9b1 · 650b9b1
1 parent d74468c
commit 650b9b1
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 5 deletions.
diff --git a/global_planner/cfg/GlobalPlanner.cfg b/global_planner/cfg/GlobalPlanner.cfg
@@ -28,7 +28,7 @@ orientation_enum = gen.enum([
               "Forward or backward depending on what requires less turning from start to path and from path to goal"),
     gen.const("Omni", int_t, 8,
               "Keep the same orientation as the previous point, except for the goal orientation"),
-    gen.const("LeftRightward", int_t, 9,
+    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")
 

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, OMNI, LEFTRIGHTWARD };
+enum OrientationMode { NONE, FORWARD, INTERPOLATE, FORWARDTHENINTERPOLATE, BACKWARD, LEFTWARD, RIGHTWARD, BIDIRECTIONAL, OMNI, SIDEWARD };
 
 class OrientationFilter {
     public:

diff --git a/global_planner/src/orientation_filter.cpp b/global_planner/src/orientation_filter.cpp
@@ -97,7 +97,7 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
                 }
             }
             break;
-        case LEFTRIGHTWARD:
+        case SIDEWARD:
             for (int i = 0; i < n - 1; i++) {
                 setAngleBasedOnPositionDerivative(path, i);
             }
@@ -109,8 +109,9 @@ void OrientationFilter::processPath(const geometry_msgs::PoseStamped& start,
                 const double rotation_to_path = angles::shortest_angular_distance(start_orientation, start_path_theta);
                 const double rotation_to_goal = angles::shortest_angular_distance(start_orientation, goal_path_theta);
 
-                bool prefer_leftward = std::fabs(rotation_to_path - M_PI_2) + std::fabs(rotation_to_goal - M_PI_2) < 
-                                       std::fabs(rotation_to_path + M_PI_2) + std::fabs(rotation_to_goal + M_PI_2);
+                bool prefer_leftward = 
+                    std::fabs(angles::normalize_angle(rotation_to_path - M_PI_2)) + std::fabs(angles::normalize_angle(rotation_to_goal - M_PI_2)) < 
+                    std::fabs(angles::normalize_angle(rotation_to_path + M_PI_2)) + std::fabs(angles::normalize_angle(rotation_to_goal + M_PI_2));
 
                 for (int i = 0; i < n - 1; i++) {
                     const double path_orientation = tf2::getYaw(path[i].pose.orientation);