Fix: elementary path sometimes are not computed due to this condition

[hbanzhaf]

Could you please check the paper: T. Fraichard and A. Scheuer, "From Reeds and Shepp's to Continuous-Curvature Paths," in IEEE Transactions on Robotics, 2004.

The additional threshold on delta should be explained there.

[Gonzalo-Mier]

Hi @hbanzhaf, sorry for the null description, I'll try it again:

The threshold is not explained there, but in "Continuous-Curvature Path Planning for Car-Like Vehicles" (https://ieeexplore.ieee.org/document/655130). In section 4.2 they said that an elementary path exists and is feasible if theta_lim is smaller than that threshold (and it gives a range for beta in that case). It doesn't say that elementary paths don't exist otherwise. This is the reason of the PR.

[hbanzhaf]

@Gonzalo-Mier: Can you give an example that shows that the current threshold is not needed? @palmieri: What do you think?

[palmieri]

I would also propose if you can show this in a test file.

[Gonzalo-Mier]

Hi @hbanzhaf and @palmieri

I've made 2 tests. On the first one, it's just doing half turn.

TEST(dubins_cc, half_turn) {
    steer::State start, end;
    start.x = 0.0;
    start.y = 0.0;
    start.theta = 0.0;
    start.kappa = 0.0;
    start.d = 0;

    end.x = 0.0;
    end.y = 2.0;
    end.theta = M_PI;
    end.kappa = 0.0;
    end.d = 0;

    CC00_Dubins_State_Space ss(10.0, 1.0, 0.05, true);
    auto controls = ss.get_controls(start, end);
    double len {0};
    for (auto&& c : controls) {
    len += c.delta_s;
  }
  EXPECT_NEAR(len, 2.0, 1.0);
}

With the threshold: len evaluates to 25.42815260869498 Without the threshold: len evaluates to 2.1094503645649643

In this test, the condition was creating the error.

Also, this is a second test where it only has to do a change of 0.01 degrees:

TEST(dubins_cc, small_angle_diff) {
    steer::State start, end;
    start.x = 0.0;
    start.y = 0.0;
    start.theta = 0.0;
    start.kappa = 0.0;
    start.d = 0;

    end.x = 2.0;
    end.y = 0.0;
    end.theta = 0.01 * M_PI / 180.0;
    end.kappa = 0.0;
    end.d = 0;

    CC00_Dubins_State_Space ss(1.0, 0.1, 0.05, true);
    auto controls = ss.get_controls(start, end);
    double len {0};
    for (auto&& c : controls) {
    len += c.delta_s;
  }
  EXPECT_NEAR(len, 2.0, 1e-2);
}

With the threshold: len evaluates to 45.242417819881339 Without the threshold: len evaluates to 35.018810103282867

Removing the threshold helps, but there is something more there.

I hope this helps. Thank you for taking the time to check it.

[Gonzalo-Mier]

Hi @hbanzhaf , did you have time to check this?

[hbanzhaf]

@Timple @vebjornjr: Could you have a look into the reported issue?

[Timple]

It looks like kind of a random number in the codebase. So I'm not against just removing it :slightly_smiling_face:

[hbanzhaf]

@Gonzalo-Mier Could you add to your test an assertion that the controls don't violate kappa_max nor sigma_max?

If that's the case, could you please visualise the denominator of the sigma_0 equation, i.e., the values of 4 * PI * pow(this->D1(0.5 * delta), 2) for delta in [0, 2*pi[. For delta = 4.5948, the denominator and thus sigma_0 should become zero, which is not a valid value for the elementary path. @palmieri @Timple: FYI.