diff --git a/global_planner/cfg/GlobalPlanner.cfg b/global_planner/cfg/GlobalPlanner.cfg index 1cf71af36c..c2ab81774f 100755 --- 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 index 544595e534..1d08e7b560 100644 --- 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 index 67bafb58a5..0f88b0188f 100644 --- 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& pat set_angle(&path[i], angle); } } - + };