SampleMani::checkcollision ignore the last pt checkcollision?

[frankchan12138]

In function SampleMani::checkcollision, you check collision for 0 ~ check_num_ -1 pt, while actually car_state_list_check_ size is check_num_ + 1 which can be inferred from the following line:

REMANI-Planner/remani_planner/path_searching/src/kino_astar.cpp

Line 233 in 2ddc9b2

car_statelist_check.push_back(end_state_temp);

Is this behavior expected, or is it a bug?
Thanks for reply！

[CK1201]

Hi, thank you for your insightful question and for carefully reviewing the code.

This is indeed the expected behavior, not a bug. Your analysis is largely correct, and I'd like to add some clarification to make the logic clearer:

The code's logic is divided into two main cases, depending on the value of has_path_ in kino_astar.cpp:

1. When has_path_ is false (A search fails to find a path for mobile base):

• As you correctly pointed out, in this scenario, the size of car_state_list_check_ is indeed check_num_ + 1 (kino_astar.cpp#L233).

• However, since no path was found, the code sets astar_succ to false (sample_mani_RRT.cpp#L49).

• This causes the program to enter the whole-body local search phase directly, and the car_state_list_check_ populated in this context is not used by the SampleMani::checkcollision function. Therefore, the size mismatch is irrelevant in this case.

2. When has_path_ is true (A search successfully finds a path for mobile base):

• In this case, car_state_list_check_ is populated with states from multiple segments (pieces) of the path. Its final size is piece_nums * check_num_ + 1 (kino_astar.cpp#L193).

• The logic here is to insert check_num_ - 1 sampled points between each pair of original path points. This is done to perform a finer, more granular collision check for the manipulator's trajectory.

• Therefore, the purpose of the SampleMani::checkcollision function is precisely to check for collisions of the manipulator—whose state is calculated via linear interpolation—between two mobile base waypoints. It checks these check_num_ - 1 intermediate states, which is why the loop from 0 to check_num_ - 1 is correct.

In summary:

The loop inside checkcollision is not intended to iterate through every point in car_state_list_check_. Instead, it is specifically designed to check the interpolated manipulator states between each pair of keyframes (the original path points). This design ensures that the manipulator's continuous trajectory is collision-free as the chassis moves.

I hope this explanation helps clarify things. Thanks again for your contribution!