code comments

[qsl123]

Hi karl: Do you have more detailed code comments? I don't understand some code. thank you very much。

[karlkurzer]

Hey, unfortunately not. What parts are you interested in specifically?

[qsl123]

Hi Karl: Thank you for your reply！I have some questions. When will DUBINS SHOT be used? How are “const float Node3D::dy[] = { 0, -0.0415893, 0.0415893}; const float Node3D::dx[] = { 0.7068582, 0.705224, 0.705224};const float Node3D::dt[] = { 0, 0.1178097, -0.1178097};” calculated from the minimum turning radius?

[karlkurzer]

Here is the line for the dubins shot: https://github.com/karlkurzer/path_planner/blob/b102a3252e0c949d7106bd85d6a0dda3b130b7ef/src/algorithm.cpp#L166

so basically if it is activated and it is in range.

The motion primitives are constant curvature curves (so, partial circles) with the maximum curvature of the vehicle with a length of more than \sqrt(2)*a with a being the side length of a cell. The three arrays hold the values for delta x, y and theta respectively.

https://github.com/karlkurzer/path_planner/blob/39705972c28008cc8f2dcf77b4af106d187667a5/src/node3d.cpp#L14-L16

[qsl123]

Hi Karl: Sorry to bother you again. // NODE POINTER Node3D nPred; Node3D nSucc; What type of pointer is this? // define list pointers and initialize lists Node3D nodes3D = new Node3D[length](); Node2D nodes2D = new Node2D[width * height](); What is the relationship between this item and the above?

[karlkurzer]

Is this still of interest, sorry for the delayed reply?

[qsl123]

Is this still of interest, sorry for the delayed reply?

sure，thank you for your reply

[qsl123]

how to show ’ALL EXPANDED 3D and 2D NODES‘ by function Visualize::publishNode3DPoses AND publishNode2DPoses ？

[peterWon]

Here is the line for the dubins shot:

https://github.com/karlkurzer/path_planner/blob/b102a3252e0c949d7106bd85d6a0dda3b130b7ef/src/algorithm.cpp#L166

so basically if it is activated and it is in range.

The motion primitives are constant curvature curves (so, partial circles) with the maximum curvature of the vehicle with a length of more than \sqrt(2)*a with a being the side length of a cell. The three arrays hold the values for delta x, y and theta respectively.

https://github.com/karlkurzer/path_planner/blob/39705972c28008cc8f2dcf77b4af106d187667a5/src/node3d.cpp#L14-L16

Hi, Karl. Thanks for your reply, But I'm still confused about how to get these 'dx,dy,dt' if I have a different turning radius with different maximum degree restrict. Looking forward your answer, Thanks.

[karlkurzer]

// R = 6, 6.75 DEG
const float Node3D::dy[] = { 0,        -0.0415893,  0.0415893};
const float Node3D::dx[] = { 0.7068582,   0.705224,   0.705224};
const float Node3D::dt[] = { 0, 0.1178097, -0.1178097};

[SteveMacenski]

I'm working through that math as we speak, so I figured it would be good for posterity to explain this a little more. This is another way of looking at it with inline comments.

  // square root of two arc length used to ensure leaving current cell
  const float sqrt_2 = sqrt(2.0);
  // angle must meet 3 requirements:
  // 1) be increment of quantized bin size
  // 2) chord length must be greater than sqrt(2) to leave current cell 
  // 3) maximum curvature must be respected, represented by minimum turning angle
  // Thusly:
  // On circle of radius minimum turning angle, we need select motion primatives 
  // with chord length > sqrt(2) and be an increment of our bin size
  //
  // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size
  // Thusly: angle <= asin(sqrt(2) / (2 * R))
  float angle = asin(sqrt_2 / (2 * min_turning_angle));
  // Now make sure angle is an increment of the quantized bin size
  // And since its based on the minimum chord, we need to make sure its always larger
  const float bin_size = 2.0 * M_PI / num_angle_quantization;
  if (angle < bin_size) {
    angle = bin_size;
  } else if (angle > bin_size) {
    int increments = ceil(angle / bin_size);
    angle = bin_size * increments;
  }

  // find deflections
  // If we make a right triangle out of the chord in circle of radius 
  // min turning angle, we can see that delta X = R * sin (angle)
  float delta_x = min_turning_angle * sin(angle);
  // Using that same right triangle, we can see that the complement
  // to delta Y is R * cos (angle). If we subtract R from it, we get
  // the actual value
  float delta_y = (R * cos(angle)) - R;

  projections.clear();
  projections.reserve(3);
  projections.emplace_back(sqrt_2, 0.0, 0.0);  // Forward
  projections.emplace_back(delta_x, delta_y, angle);  // Left
  projections.emplace_back(delta_x, -delta_y, -angle);  // Right

[karlkurzer]

@SteveMacenski do you wanna make a PR for documentation you added, might be helpful for others?

[JialiangHan]

I'm working through that math as we speak, so I figured it would be good for posterity to explain this a little more. This is another way of looking at it with inline comments.

  // square root of two arc length used to ensure leaving current cell
  const float sqrt_2 = sqrt(2.0);
  // angle must meet 3 requirements:
  // 1) be increment of quantized bin size
  // 2) chord length must be greater than sqrt(2) to leave current cell 
  // 3) maximum curvature must be respected, represented by minimum turning angle
  // Thusly:
  // On circle of radius minimum turning angle, we need select motion primatives 
  // with chord length > sqrt(2) and be an increment of our bin size
  //
  // chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size
  // Thusly: angle <= asin(sqrt(2) / (2 * R))
  float angle = asin(sqrt_2 / (2 * min_turning_angle));
  // Now make sure angle is an increment of the quantized bin size
  // And since its based on the minimum chord, we need to make sure its always larger
  const float bin_size = 2.0 * M_PI / num_angle_quantization;
  if (angle < bin_size) {
    angle = bin_size;
  } else if (angle > bin_size) {
    int increments = ceil(angle / bin_size);
    angle = bin_size * increments;
  }

  // find deflections
  // If we make a right triangle out of the chord in circle of radius 
  // min turning angle, we can see that delta X = R * sin (angle)
  float delta_x = min_turning_angle * sin(angle);
  // Using that same right triangle, we can see that the complement
  // to delta Y is R * cos (angle). If we subtract R from it, we get
  // the actual value
  float delta_y = (R * cos(angle)) - R;

  projections.clear();
  projections.reserve(3);
  projections.emplace_back(sqrt_2, 0.0, 0.0);  // Forward
  projections.emplace_back(delta_x, delta_y, angle);  // Left
  projections.emplace_back(delta_x, -delta_y, -angle);  // Right

when i read original paper, one section said same cell expansion, if we consider that, chord length doesn`t need to be largeer than sqrt(2)