Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add FixedOrientation (Omni) and Sideward orientation modes #89

Merged
merged 7 commits into from
Nov 8, 2023
Merged

Add FixedOrientation (Omni) and Sideward orientation modes #89

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
7fad1d8
add orientations
renan028 Nov 3, 2023
2d22859
fix sideways
renan028 Nov 3, 2023
d74468c
fix left right
renan028 Nov 6, 2023
650b9b1
fix angle
renan028 Nov 6, 2023
011d0c4
copy from bidirectional
renan028 Nov 6, 2023
041a4ba
fix cost
renan028 Nov 7, 2023
3829dab
change var name
renan028 Nov 7, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion global_planner/cfg/GlobalPlanner.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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("Omni", int_t, 8,
renan028 marked this conversation as resolved.
Show resolved Hide resolved
"Keep the same orientation as the previous point, except for the goal orientation"),
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)
Expand Down
2 changes: 1 addition & 1 deletion global_planner/include/global_planner/orientation_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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, OMNI, SIDEWARD };

class OrientationFilter {
public:
Expand Down
32 changes: 30 additions & 2 deletions global_planner/src/orientation_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -97,6 +97,29 @@ 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_path_theta = tf2::getYaw(path[num_skips].pose.orientation);
const double goal_path_theta = tf2::getYaw(path[n - 1 - num_skips].pose.orientation);
const double start_orientation = tf2::getYaw(start.pose.orientation);
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);
renan028 marked this conversation as resolved.
Show resolved Hide resolved

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));
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

F: I think the second time, the sign of the half rotation needs to be inverted, no? I.e.

Suggested change
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));
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));

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It should be inverted, that is correct, thanks. The robot will need to undo the rotation.

image

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nice picture. Is that the plot of

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));

I suppose? Is there some intuitive way to see that this is indeed the correct result?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes. that is the plot of the function:

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));

hm, I think the best way to understand is by visualizing the start/goal in circle quadrant:

image

The cases we want leftward/rightward (figures on the left), we can see that the solution is the correct one.
The other cases (when we switch orientation by a lot) it doesn't matter much which one we should choose since the "cost" is basically the same (for both leftward or rightward), and that is why we have this "frog" graph.
My concern is with the switching part, which can lead to some oscillation, maybe


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);
Expand All @@ -109,6 +132,12 @@ 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 OMNI:
path[0].pose.orientation = start.pose.orientation;
for(int i=1;i<n-1;i++){
set_angle(&path[i], tf2::getYaw(path[i-1].pose.orientation));
}
renan028 marked this conversation as resolved.
Show resolved Hide resolved
break;
case INTERPOLATE:
path[0].pose.orientation = start.pose.orientation;
interpolate(path, 0, n-1);
Expand Down Expand Up @@ -160,7 +189,6 @@ void OrientationFilter::interpolate(std::vector<geometry_msgs::PoseStamped>& pat
double angle = start_yaw + increment * i;
set_angle(&path[i], angle);
}
}

}

};