Add FixedOrientation (Omni) and Sideward orientation modes

[renan028]

Description

wrike: https://www.wrike.com/open.htm?id=1243708620
In Taisei project, customer requires Takumi robot to prefer to move sideways after picking the payload.
It is safer to move sideways after picking, because robot grows a lot in y-axis (image below). It is safer to move in the direction of the minimal sweep for collision avoidance.

There are different ways to address that:

1. Rotate the base_link frame; or velocities / odom, etc
2. Change the GlobalPlanner to generate the correct path
3. Tune TEB
   The (1) solution is a bit hacky and I think it would cause some issues. Since everything (perception, localization and navigation nodes) relies in base_link frame, changing it dynamically seems dangerous. And rotating the velocities may cause other issues too.
   The (3) seems to be the easiest one, but it did not work as expected (as explained below).
   The (2) solution was implemented.

We attempted to change TEB to prefer moving sideways without changing global planner:

1. global_plan_overwrite_orientation to True so TEB can choose the best orientation for minimal time
2. max_vel_x to 0.06 and max_vel_y to 0.5 so that vel_y is chosen
   However, TEB does not rotate the robot and moves sideways. It continues to move forwards/backward in a very small speed.

After changing the GlobalPlanner logic by adding a new mode (LeftRightWard), TEB manages to use the correct orientation.

leftrightward-2023-11-06_13.09.05.mp4

[ChristofDubs]

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

[renan028]

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

[ChristofDubs]

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?

[renan028]

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:

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