Ignore Final Pose Orientation

[Jmeyer1292]

Greetings,

I've been looking at applying TEB to an application that:

1. Requires tight quarters maneuvering
2. Does not care about terminal waypoint orientation
3. Really cannot tolerate reverse driving

The robot in question:

1. Is diff drive
2. Has a circular footprint

The robot in question has a circular footprint, so turn-in-place is usually an acceptable behavior. I have essentially used the rotate in place shim to get the desired "start of path" behavior: When new goals come in the robot first rotates to face it. It would be a nice complement to this behavior and would make it easier to achieve no-reverse driving, if the robot did not care about the terminal waypoint orientation during TEB's execution.

In some sense, I want to declare only the X/Y position of the final waypoint as fixed and let the theta value be free. Looking into the tutorials and documentation, I did not see anything that led me to believe I could do this. In code, I see where I might provide my own estimate of the final orientation for a given iteration, however, leaving up to the optimizer seems infeasible as a vertex pose is either completely fixed or completely free.

Assuming that a slight code divergence is acceptable for this trial, how might the experts here recommend I proceed? Are there G2O options I might exploit, such as bounds on a variable instead of "freezing" them?

Off the top of my head, some things I was considering:

1. Refactor VertexPose to be composed of separate orientation & position vertices that might be fixed independently.
2. Solve at the seed level: replace global planner (currently using navfn) with something like a lattice planner to generate kinodynamically feasible paths. Tune the costs such that it doesn't expand that path into a larger reverse segment.
3. Remove the fix on the final position and replace it with a soft constraint: some via point like edge that pulls the final goal position toward the target position. Might be hard to balance with other costs, but on the plus side is probably easy (comparatively) to insert.
4. ???

I apologize for the open ended question, and I'm happy to direct this some place more appropriate, such as a discourse, if such a place exists.

References:
Someone asking for rotation at the start: #370

[Dhivin]

I partially solved this problem by setting the goal_pose_vertex(when robot is going to reach the goal) as marginalized.

void TimedElasticBand::setPoseVertexMarginalized(int index, bool status){
  ROS_ASSERT(index<sizePoses());
  pose_vec_.at(index)->setMarginalized(status);   
}

@RainerKuemmerle what are your thoughts on doing this?