RRT 扩展新节点小错误

[kun1224]

1. 没有乘以步长,改动如下

   else
   { // 否则，沿着x_near与x_rand的连线方向上扩展固定步长u，得到新节点 x_{new}
       new_x = from_node->x + extend_length * cos(dist_angle[1]);
       new_y = from_node->y + extend_length * sin(dist_angle[1]);
   }

[CHH3213]

yep, it's a known bug, several moths ago I have fixed them in my python codebase version, but I forgot to fix them in CPP version, thanks for reminder.

[kun1224]

ok，please look the other two issue i addressed if you have time. i am learning your code recently