代码不能到达给定位姿，将注释掉的删除起始点的代码取消注释就可以，这是为啥？

[David3665]

我这样设置的起点和终点： 但是执行的结果目标点的位置是错误的： 如果把此句取消注释再运行： 结果如图： 可见结果基本是对的，请问这是什么原因？

[wanghuohuo0716]

@David3665 你好，我的代码似乎没有注释该行代码！请查看截图！

[David3665]

@wanghuohuo0716 作者好！这个问题我解决了，是第25行与第35行的isok标志位发生混用导致部分路径点被删除，我加了个判断解决了这个问题 此外，您代码中生成RS曲线的FindRS函数在产生CCCC类的轨迹时，当移动距离超过2*rmin时会产生错误的结果，我换用matlab自带的RS生成函数解决了这个问题，我写的替换函数如下：

[wanghuohuo0716]

感谢 @David3665 提供的修改意见，

①关于源代码中isok标志位发生混用问题，完整的修改代码是否如下

    while ~isempty(Open)
        % pop the least cost node from open to close
        [wknode,Open] = PopNode(Open,cfg);
        [isok1,idx] = inNodes(wknode,Close);
        
        % 判断是否在Close集合内
        if isok1
            Close(idx) = wknode;
        else
            Close = [Close, wknode];
        end

        % 以wknode为根节点生成搜索树，使用Reeds-Shepp方法基于车辆单轨模型进行运动学解析拓展子结点
        [isok2,path] = AnalysticExpantion([wknode.x,wknode.y,wknode.theta],End,veh,cfg);
        if  isok2
            %把wknode从idx移到Close集合最后面
            if isok1
                Close(end+1) = wknode;
                Close(idx) = [];
            else
                [x,y,th,D,delta] = getFinalPath(path,Close,veh,cfg);
            end
            break % 如果能直接得到RS曲线，则跳出while循环
        end
        [Open,Close] = Update(wknode,Open,Close,veh,cfg); % 使用
    end

---

②关于RS曲线的FindRS函数在产生CCCC类的轨迹错误问题，我在测试过程， 下面这句代码

reedsConnObj = ReedsSheppConnection;

似乎无法运行，提示错误：

未定义函数或变量 'ReedsSheppConnection'。

出错 FindRSPath_plus (line 5)
    reedsConnObj = ReedsSheppConnection;

---

不知您可否提交一份完整的测试代码的PR到这个仓库呢？

非常感谢！！！

[David3665]

@wanghuohuo0716 作者您好，已上传 https://github.com/wanghuohuo0716/hybrid_A_star/pull/8 matlab需要安装Navigation Toolbox