2022-4-5 ompl 分段实时规划 丢失未解决

保留path的已运动部分，append 从新起点出发的 后序 path

           globalsolved = globalsetup.solve(0.005);
            if(globalsetup.haveExactSolutionPath())
            {
                temppath.clear();
                temppath = globalsetup.getSolutionPath();
                templen = temppath.length();
                if(templen<length && templen<globalpath.length())
                {
                    length = templen;

                    std::cout << "update shortest path......  " << length << std::endl;

                    /* remove old path and append new one */
                    temppath.interpolate(10-m_pathindex);
                    cout << " new path : "<< temppath.getStateCount() <<endl;
                    temppath.print(cout);

//                    ob::ScopedState<OmplStateSpace> currentstate(globalsi);
//                    currentstate = globalpath.getState(m_pathindex);

                    cout << " current planning's start state: "<<endl;
                    currentstate.print(cout);

                    cout << " old global path : "<< globalpath.getStateCount()<< endl;
                    globalpath.print(cout);

                    /* keep current state's former path point */
                    globalpath.keepBefore(globalpath.getState(m_pathindex-1));
                    cout << " keep part of old path : "<< globalpath.getStateCount() <<endl;
                    globalpath.print(cout);

                    globalpath.append(temppath);
                    cout << " append new path to old path : "<< globalpath.getStateCount()<<endl;
                    globalpath.print(cout);

                }
            }

问题：新的setup 求解是从新起点出发，但是得到的path仍是从旧起点的。append总路径覆盖了新起点为旧的起点。

解决：setup 不仅需要clear start state ，还需要clear ，清除之前的path solution。

新的path 从新起点出发

新的 整体path

问题：Keepbefore 有时出错，保存的前序路径少一？

解决：弥补：
get state可以得到当前运行中路径段的终点，即当前规划部分的起点状态，存入全局变量endstate，当Keepstate 中状态数量少一时，先append endstate，再append 新path。测试有效，丢失状态原因未解决。