Steering Function subtract control assertion error, cc dubins state space

[vinnnyr]

steering_functions/src/hc_cc_state_space/paths.cpp:283: steering::Control steering::subtract_control(const steering::Control&, const steering::Control&): Assertion `sgn(control1.delta_s) * control1.sigma == sgn(control2.delta_s) * control2.sigma' failed.

While developing on top of this library, I am occasionally failing this assertion error. It seems to be when I try to generate plans between two states that are relatively close together (but not always).

CC Dubins State Space.

I will try to produce a minimal example when possible, but I wanted to raise this issue in case there can be discussion in the meantime.

Cheers!

Vinny

[vinnnyr]

Debugger says it's coming from

      // check if start and goal state are equal
      if (state_equal(start_state, end_state))
      {
        **Control control = subtract_control(start_control, end_control);**
        distances.push_back(fabs(control.delta_s));
      }

[vinnnyr]

i have temporarily resolved the issue upstream by assuring the start and end states have euclidean distance at least 2x the discretization of the dubins state space