c++ - 如何从ompl :: base *goalptr中获得目标位置？

Question

我正在使用ompl进行 3D 路径规划。请指导如何从该行接收的目标指针中获取目标位置（x，y，z）。

const ob::Goal *Goalptr = pdef->getGoal().get();

pdef问题定义在哪里。并且ob路径ompl::base规划器正在规划路径，但我无法实现这个目标。

我应该怎么做这样的事情：

x= Goalptr[0];
y= Goalptr[1];
z= Goalptr[2];

或者我应该Goalptr先把它转换成其他类型，然后再做。请帮忙。

Answer 1

所以这篇文章帮助我解决了这个问题。我使用此代码来获得目标位置。

std::vector<double> reals;
space->copyToReals(reals, pdef->getGoal()->as<ob::GoalState>()->getState());

然后我们在 (reals[0], reals 1 , reals[2])中得到目标位置 (x,y,z )。

感谢 Christopher Oezbek 和其他尝试过的人。