题目 RRT
利用Rapidly Exploring Random Tree 寻找不碰到障碍物的路径。机器人可以向前任何方向在以给出的最大距离移动。只要遇见障碍就停止。
准备
2.1 求第一问
补充Funktion get_nearest_node_index
2.2 求第二问
Funktion check_collision，检查是否碰撞
2.3 求第三问
Funktion local_planner 机器人从已给位置from_pos移动到to_pos.机器人应该在每个numSteps=3步骤中进行最大的stepSize=0.33. 如果其中有碰撞，机器人则停在上一个有效位置。目标位置到达后，机器人精准的停留在目标点上。
2.4 RRT
2.5 补充code