code comments
qsl123 opened this issue · comments
Hi karl:
Do you have more detailed code comments? I don't understand some code.
thank you very much。
Hey, unfortunately not. What parts are you interested in specifically?
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?
Here is the line for the dubins shot:
path_planner/src/algorithm.cpp
Line 166 in b102a32
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.
Lines 14 to 16 in 3970597
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 Node3Dlength;
Node2D* nodes2D = new Node2Dwidth * height;
What is the relationship between this item and the above?
Is this still of interest, sorry for the delayed reply?
Is this still of interest, sorry for the delayed reply?
sure，thank you for your reply
how to show ’ALL EXPANDED 3D and 2D NODES‘ by function Visualize::publishNode3DPoses AND publishNode2DPoses ？
Here is the line for the dubins shot:
path_planner/src/algorithm.cpp
Line 166 in b102a32
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.
Lines 14 to 16 in 3970597
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.
// 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};
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
@SteveMacenski do you wanna make a PR for documentation you added, might be helpful for others?
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)