* theta对2pi取模运算 * @param theta * @return */

* theta对2pi取模运算 * @param theta * @return */

* 角度归一化到【-PI,PI】 * @param angle * @return */

* 角度归一化 * @param angle * @return */

* 角度归一化到【-PI,PI】 * @param angle * @return */

* Determine 判断是否有障碍物 (using), 使用线段与圆形的相交性方法来实现(general), * 对搜索会更加困难，这里便不考虑效率问题了... * @param node * @return */

* Determine 判断是否有障碍物 (using), 使用线段与圆形的相交性方法来实现(general), * 对搜索会更加困难，这里便不考虑效率问题了... * @param node * @return */

* 画圆 * @param x * @param y * @param size * @param color */

* 画圆 * @param x * @param y * @param size * @param color */

* 画图 * @param current */

* 画图 * @param current */

definitely remove this line

* 重置 */

* 布线 * @param new_node * @param near_inds */

* 以（100-goal_sample_rate）%的概率随机生长，(goal_sample_rate)%的概率朝向目标点生长 * @return 生成的节点 */

* 以（100-goal_sample_rate）%的概率随机生长，(goal_sample_rate)%的概率朝向目标点生长 * @return 生成的节点 */

* 设置控制量边界 * @param upper * @param lower */

* 设置pid参数 * @param kp * @param ki * @param kd */

* 设置累计误差 * @param sum_error */

* 设置目标 * @param target */

* 连线方向扩展固定步长查找x_new * @param from_node x_near * @param to_node x_rand * @param extend_length 扩展步长u. Defaults to float("inf"). * @return */

* 连线方向扩展固定步长查找x_new * @param from_node x_near * @param to_node x_rand * @param extend_length 扩展步长u. Defaults to float("inf"). * @return */

* SCS * @param x * @param y * @param phi * @param paths * @param step_size * @return */

* SLS * @param x * @param y * @param phi * @return */

Actually, is there any reason not to call this automatically for every plot?

* 轨迹评价函数,评价越高，轨迹越优 * @param state 当前状态---x,y,yaw,v,w * @param goal 目标点位置，[x,y] * @param obstacle 障碍物位置，dim:[num_ob,2] * @return 最优控制量、最优轨迹 */

* 轨迹推算 * @param state 当前状态---x,y,yaw,v,w * @param v 当前时刻线速度 * @param w 当前时刻线速度 * @return 推算后的轨迹 */

* 判断节点是否有效，即是否超出边界和碰到障碍物 * @param node * @return */

* 判断节点是否有效，即是否超出边界和碰到障碍物 * @param node * @return */