2022-4-25 qt realtime 2d planning debug

c++出现double free or corruption (fasttop)

Linux 下 segmentation fault(段错误) 总结
在LIinux 下C/C++中，出现段错误很多都是有指针造成的，指针声明后没有内容的存储空间，当你不指向指定的内存空间时，就会出现segmentation fault(段错误)，这种情况往往能编译通过的，但是运行时就会出现在段错误。

段错误segmentation fault，信号SIGSEGV，是由于访问内存管理单元MMU异常所致，通常由于无效内存引用，如指针引用了一个不属于当前进程地址空间中的地址，操作系统便会进行干涉引发SIGSEGV信号产生段错误。

C++中try、catch 异常处理机制

int divide(int x, int y )           // 抛异常
{
	if (y ==0)
	{
		throw x;
	}
	return x/y;
}
 
void main41()
{
	Try                接异常
	{
		cout << "8/2 = " << divide(8, 2) << endl;
		cout << "10/0 =" << divide(10, 0) << endl;
	}
	catch (int e)
	{
		cout << "e" << " is divided by zero!" << endl;
	}
	catch(...)
	{
		cout << "未知异常" << endl;
	}
	
	cout << "ok" << endl;
 	system("pause");
	return ;
}

C++ 异常处理

线程一改变路径点索引，线程二未更新对应状态到新的索引值，而是直接应用了旧的状态来匹配新索引，造成序列总长度不符合限制。assert 保证数据序列长度还是很有用的。

改：
索引更新和状态更新都放到一个线程里，另一个线程使用状态，不更新状态。
互斥锁未测试。
更新：还是用了互斥锁把两大块都括起来了，保证数据同步

保留从 距离最近的状态开始，但是当前状态不是最近？？

 void ompl::geometric::PathGeometric::keepBefore(const base::State *state)
 {
     int index = getClosestIndex(state);
     if (index >= 0)
     {
         if (index > 0 && (std::size_t)(index + 1) < states_.size())
         {
             double b = si_->distance(state, states_[index - 1]);
             double a = si_->distance(state, states_[index + 1]);
             if (b < a)
                 --index;
         }
         if ((std::size_t)(index + 1) < states_.size())
         {
             for (std::size_t i = index + 1; i < states_.size(); ++i)
                 si_->freeState(states_[i]);
             states_.resize(index + 1);
         }
     }
 }
  
 int ompl::geometric::PathGeometric::getClosestIndex(const base::State *state) const
 {
     if (states_.empty())
         return -1;
  
     int index = 0;
     double min_d = si_->distance(states_[0], state);
     for (std::size_t i = 1; i < states_.size(); ++i)
     {
         double d = si_->distance(states_[i], state);
         if (d < min_d)
         {
             min_d = d;
             index = i;
         }
     }
     return index;
 }
  
 void ompl::geometric::PathGeometric::clear()
 {
     freeMemory();
     states_.clear();
 }

根据保留之后的长度判断当前状态是否被保留，否则再append进来。