[feature request] add penalty for switching forward/reverse (carlike)

[VRichardJP]

Currently, teb_local_planner assumes it is possible to instantly switch from going forward to going backward (and from backward to forward) with no penalty (e.g. with constant acceleration from +1.0 to -1.0)
However for carlike robots it is not the case most of the time: because of the mechanical gear change, the robot needs first to stop, handle the gear change then can drive backward.

Because of that, the teb_local_planner may select very impracticable paths for the carlike robot.
For instance, if the robot is asked to turn around in place, the planner may find that doing many mini-turns going back and forth in place is a good solution, while a more practical (and way faster) way would be to do some kind of 3-point turn.

For example, this problem could be solved by adding a new parameter "reverse_gear_switch_time", which would be added to the path time travel whenever the robot's velocity would change sign.

[VRichardJP]

Here is a very simple implementation of what could be an EdgeGearChange:

void computeError()
  {
    ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeGearChange()");
    const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
    const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
    const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);

    // ORIENTATION
    const Eigen::Vector2d diff1 = pose2->position() - pose1->position();
    const Eigen::Vector2d diff2 = pose3->position() - pose2->position();

    _error[0] = diff1.dot(diff2) < 0.0 ? cfg_->robot.gear_change_time : 0.0;
  }

used in the AddEdgesGearChange in optimal_planner.cpp

if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot
  {
    Eigen::Matrix<double,1,1> information;
    information.fill(cfg_->optim.weight_gear_change);
    
    // add the usual acceleration edge for each tuple of three teb poses
    for (int i=0; i < n - 2; ++i)
    {
      EdgeGearChange* gear_change_edge = new EdgeGearChange;
      gear_change_edge->setVertex(0,teb_.PoseVertex(i));
      gear_change_edge->setVertex(1,teb_.PoseVertex(i+1));
      gear_change_edge->setVertex(2,teb_.PoseVertex(i+2));
      gear_change_edge->setInformation(information);
      gear_change_edge->setTebConfig(*cfg_);
      optimizer_->addEdge(gear_change_edge);
    }
  }

where robot.gear_change_time represents the time required for the robot to switch between forward and backward (eg. 2 seconds), and weight_gear_change the optimization weight for satisfying the constraint (eg. 100)

I have tested the result with a carlike robot in simulation, asking the planner to move behind and reverse the vehicle orientation.

Without the extra penalty, I often get this kind of "star-shape" turn around plans:

With the extra penality, I am more likely to get clean paths like this:

[LotfiZ]

@VRichardJP Nice work Richard, is there a possibility to share with me the implementation to test it and give some feedback ?

[VRichardJP]

Hi, sorry I don't have much time to make a clean pull request, but the piece of code I showed above is pretty much the whole implementation, so it should not be very difficult to reproduce. I really just followed the existing code design to fit in the new function/variables/parameters

[LotfiZ]

Thank you Vincent, i'll implement and test it and give some feedback !