修复了标志位复用造成的Node被删除问题

src/FindRSPath_plus.m

@@ -0,0 +1,17 @@
+function path = FindRSPath_plus(x,y,phi,veh)
+    rmin = veh.MIN_CIRCLE; %minimum turning radius
+    x = x/rmin;
+    y = y/rmin;
+    reedsConnObj = reedsSheppConnection;
+    startPose = [0 0 0];
+    goalPose = [x y phi];
+    [pathSegObj,~] = connect(reedsConnObj,startPose,goalPose);
+    test = pathSegObj{1};
+    type = [];
+    value = zeros(5,1);
+    for i = 1:5
+        type = [type,test.MotionTypes{i}];
+        value(i) = test.MotionDirections(i)*test.MotionLengths(i);
+    end
+    path = RSPath(type,value(1),value(2),value(3),value(4),value(5));
+end

src/HybridAStar.m

@@ -22,21 +22,24 @@
     while ~isempty(Open)
         % pop the least cost node from open to close
         [wknode,Open] = PopNode(Open,cfg);
-        [isok,idx] = inNodes(wknode,Close);
+        [isok1,idx] = inNodes(wknode,Close);
 
         % �ж��Ƿ���Close������
-        if isok
+        if isok1
             Close(idx) = wknode;
         else
             Close = [Close, wknode];
         end
 
         % ��wknodeΪ���ڵ�������������ʹ��Reeds-Shepp�������ڳ�������ģ�ͽ����˶�ѧ������չ�ӽ��
-        [isok,path] = AnalysticExpantion([wknode.x,wknode.y,wknode.theta],End,veh,cfg);
-        if  isok
+        [isok2,path] = AnalysticExpantion([wknode.x,wknode.y,wknode.theta],End,veh,cfg);
+        if  isok2
             %��wknode��idx�Ƶ�Close���������
-            Close(end+1) = wknode;
-            Close(idx) = [];
+            if isok1
+                Close(end+1) = wknode;
+                Close(idx) = [];
+            else
+            end
             [x,y,th,D,delta] = getFinalPath(path,Close,veh,cfg);
             break % �����ֱ�ӵõ�RS���ߣ�������whileѭ��
         end