C++ C2436&x27；{ctor}'；：构造函数初始值设定项列表中的成员函数或嵌套类
我真的不明白编译器错误中的成员函数是什么意思C++ C2436&x27；{ctor}'；：构造函数初始值设定项列表中的成员函数或嵌套类,c++,constructor,initializer-list,C++,Constructor,Initializer List,我真的不明白编译器错误中的成员函数是什么意思 C2436'{ctor}”：构造函数初始值设定项列表中的成员函数或嵌套类 使用Visual C++ +/P> 在my SubnearestNeights.hpp中写入： template<typename _T> class SubNearestNeighbors : public ompl::NearestNeighbors<_T> { public: typedef std::function<dou
C2436'{ctor}”：构造函数初始值设定项列表中的成员函数或嵌套类使用Visual C++ +/P> 在my SubnearestNeights.hpp中写入：
template<typename _T>
class SubNearestNeighbors : public ompl::NearestNeighbors<_T>
{
public:
typedef std::function<double(const _T&, const _T&)> DistanceFunction;
SubNearestNeighbors(const ob::SpaceInformationPtr &si)
:
PathPlannerOMPLServer::kdRRTConnect::kdRRTConnect()
{}
SubNearestNeighbors()// here is the member function cause the error <----
: // but it doesn't have any void in it? |
PathPlannerOMPLServer::kdRRTConnect::kdRRTConnect() |
{} // error get here, it say member func. or nested class in constructor|
// initialize list, while compiling class template member function |
//'SubNearestNeighbors<PathPlannerOMPLServer::kdRRTConnect::Motion*> |
//::SubNearestNeighbors(void) ---------------------------------------
~SubNearestNeighbors()
{
}
//...more other function
PathPlannerOMPLServer::kdRRTConnect::kdRRTConnect()
:
ompl::base::Planner((const base::SpaceInformationPtr &)*((base::SpaceInformationPtr*)(unsigned char*)0), "kdRRTConnect") {
typedef std::shared_ptr < ompl::NearestNeighbors<Motion*>> nn_ptr;
nn_ptr nn_;
nn_->setDistanceFunction(std::bind(&kdRRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
模板
类SubnearestNeights:公共ompl:：NearestNeights
{
公众：
typedef std:：函数距离函数；
子近邻（const ob:：SpaceInformationPtr&si）
:
PathPlannerOMPLServer:：kdRRTConnect:：kdRRTConnect（）
{}
subnearestneights（）//这是导致错误nn_ptr的成员函数；
nn_ptr nn_；
nn->setDistanceFunction（std:：bind（&kdRRTConnect:：distanceFunction，this，std:：占位符：：_1，std:：占位符：：_2））；
}
有人能给我一些提示吗，初始化成员列表中的成员函数subnearestneights（void）是什么意思，但实际上它在subnearestneights中不存在任何void。谢谢@M.M和@Fei Xiang 因为_T是在公共的ompl:：nearestneights（这里是该类的链接：ompl.kavrakilab.org/nearestneights_8h_source.html）中定义的，所以只需模拟另一个类nearestneightorslinear（ompl.kavrakilab.org/nearestneights_8h_source.html）、nearestneightneights（ompl.kavrakilab.org/nearsnights_8h_source.html），NearestNeightarSqrtApprox（ompl.kavrakilab.org/nearestNeightarSqrtApprox_8h_source.html），也继承了ompl:：NearestNeights。我写模板没有其他选择 这里我给出了Ompl:：NearestNeights类的代码，我继承并重写了该类中的函数
namespace ompl
{
template <typename _T>
class NearestNeighbors
{
public:
typedef std::function<double(const _T &, const _T &)> DistanceFunction;
NearestNeighbors() = default;
virtual ~NearestNeighbors() = default;
virtual void setDistanceFunction(const DistanceFunction &distFun)
{
distFun_ = distFun;
}
const DistanceFunction &getDistanceFunction() const
{
return distFun_;
}
virtual bool reportsSortedResults() const = 0;
virtual void clear() = 0;
virtual void add(const _T &data) = 0;
virtual void add(const std::vector<_T> &data)
{
for (const auto &elt : data)
add(elt);
}
virtual bool remove(const _T &data) = 0;
virtual _T nearest(const _T &data) const = 0;
virtual void nearestK(const _T &data, std::size_t k, std::vector<_T> &nbh) const = 0;
virtual void nearestR(const _T &data, double radius, std::vector<_T> &nbh) const = 0;
virtual std::size_t size() const = 0;
virtual void list(std::vector<_T> &data) const = 0;
protected:
DistanceFunction distFun_;
};
}
名称空间ompl
{
模板
类最近邻
{
公众：
typedef std:：函数距离函数；
nearestneights（）=默认值；
virtual~nearestneights（）=默认值；
虚拟void setDistanceFunction（常量距离函数和距离函数）
{
distFun=distFun；
}
常量距离函数&getDistanceFunction（）常量
{
返回distFun；
}
虚拟布尔ReportsOrderedResults（）常量=0；
虚空清除（）=0；
虚拟空添加（常量和数据）=0；
虚拟空添加（常量标准：：向量和数据）
{
用于（常数自动和elt：数据）
增加（英语教学）；
}
虚拟布尔删除（常量和数据）=0；
虚拟最近（常数和数据）常数=0；
虚拟空位最接近k（常数和数据，标准：：大小，标准：：向量和nbh）常数=0；
虚拟近空（常数和数据，双半径，标准：：矢量和nbh）常数=0；
虚拟std:：size\u t size（）常量=0；
虚拟无效列表（标准：：向量和数据）常数=0；
受保护的：
距离函数distFun；
};
}
template<typename _T>
class SubNearestNeighbors: public ompl::NearestNeighbors<_T>
{
public:
SubNearestNeighbors(const ob::SpaceInformationPtr &si)
:
PathPlannerOMPLServer::kdRRTConnect::kdRRTConnect(){}
SubNearestNeighbors()
:
PathPlannerOMPLServer::kdRRTConnect::kdRRTConnect(){}
~SubNearestneighbors()
{
}
// All the functions below are just override all of the pure virtual function in ompl::NearestNeighbors<_T>
void clear()
{
motions.clear();
}
void list(std::vector<_T> &data) const
{
typename PathPlannerOMPLServer::kdRRTConnect::Motion;
data = motions;
}
std::size_t size() const
{
return motions.size();
}
void add(const _T &data)
{
typename PathPlannerOMPLServer::kdRRTConnect::Motion;
motions.push_back(data);
}
bool remove(const _T &data)
{
typename PathPlannerOMPLServer::kdRRTConnect::Motion;
if (!motions.empty())
for (int i = motions.size() - 1; i >= 0; --i)
if (motions[i] == data)
{
motions.erase(motions.begin() + i);
return true;
}
return false;
}
_T nearest(const _T &data) const
{
typename PathPlannerOMPLServer::kdRRTConnect::Motion;
// here is about 1000 lines code
// I am sure here works, because i have test it in main.cpp and that's really
// what i want. The logic are just get n random point, which are the around
// small room of state (rstate in RRTConnect )-> build the KD Tree -> give a
// searchPoint to find the nearestNeighbor point in KDTree -> convert the
// nearestNeighbor point in state -> make a new motion *nmotion -> save it i
//in motion->state
return motion;
}
protected:
std::vector<_T> motions;
};
模板
类SubnearestNeights:公共ompl:：NearestNeights
{
公众：
子近邻（const ob:：SpaceInformationPtr&si）
:
PathPlannerOMPLServer:：kdRRTConnect:：kdRRTConnect（）{}
次近邻（）
:
PathPlannerOMPLServer:：kdRRTConnect:：kdRRTConnect（）{}
~subnearestneights（）
{
}
//下面的所有函数都只是覆盖ompl:：NearestNeights中的所有纯虚函数
无效清除（）
{
运动。清晰（）；
}
无效列表（标准：：矢量和数据）常数
{
typename PathPlannerOMPLServer:：kdRRTConnect:：Motion；
数据=运动；
}
std:：size\u t size（）常量
{
返回运动。size（）；
}
无效添加（常量和数据）
{
typename PathPlannerOMPLServer:：kdRRTConnect:：Motion；
运动。推回（数据）；
}
布尔删除（常量和数据）
{
typename PathPlannerOMPLServer:：kdRRTConnect:：Motion；
如果（！motions.empty（））
对于（int i=motions.size（）-1；i>=0；--i）
if（运动[i]==数据）
{
motions.erase（motions.begin（）+i）；
返回true；
}
返回false；
}
_T最近（常数和数据）常数
{
typename PathPlannerOMPLServer:：kdRRTConnect:：Motion；
//这里有大约1000行代码
//我确信这是可行的，因为我已经在main.cpp中对它进行了测试，这真的很好
//我想要的，逻辑就是得到n个随机点，这是周围的
//小型状态空间（RRTConnect中的rstate）->构建KD树->给出一个
//searchPoint在KDTree中查找最近的邻居点->转换
//状态中的最近邻点->创建新运动*n运动->保存它i
//运动->状态
返回运动；
}
受保护的：
矢量运动；
};
namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace PathPlannerOMPLServer
{
class kdRRTConnect :public ompl::base::Planner
{
public:
//brief constructor
kdRRTConnect(const ompl::base::SpaceInformationPtr &si);
kdRRTConnect();
virtual ~kdRRTConnect();
virtual void getPlannerData(ompl::base::PlannerData &data)const;
virtual ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc);
virtual void clear();
void setRange(double distance)
{
maxDistance_ = distance;
}
double getRange() const
{
return maxDistance_;
}
template<template<typename T> class NN>
void setNearestNeighbors()
{
tStart_.reset(new NN<Motion*>());
tStart_->setDistanceFunction(std::bind(&kdRRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
tGoal_.reset(new NN<Motion*>());
tGoal_->setDistanceFunction(std::bind(&kdRRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
virtual void setup();
//define Motion same as RRTConnect, but in public
class Motion
{
public:
Motion() : root(nullptr), state(nullptr), parent(nullptr)
{
parent = nullptr;
state = nullptr;
}
Motion(const ompl::base::SpaceInformationPtr &si) : root(nullptr), state(si->allocState()), parent(nullptr)
{
}
~Motion()
{
}
const ompl::base::State *root;
ompl::base::State *state;
Motion *parent;
};
protected:
// a Sub nearest-neighbor datastructure represents a tree of motions
typedef std::shared_ptr< ompl::NearestNeighbors<Motion*>> TreeData;
//brief Info. attached to gowing a tree motions(use internally)
struct TreeGrowingInfo
{
ompl::base::State *xstate;
Motion *xmotion;
bool start;
};
//brief The state of the tree after an attempt to extend it
enum GrowState
{
/// no progress has been made
TRAPPED,
/// progress has been made towards the randomly sampled state
ADVANCED,
/// the randomly sampled state was reached, d !> maxDistance
//double d = si_->distance(nmotion->state, rmotion->state); <- in growTree()
REACHED
};
//brief Free the memory allocated by this planner
void freeMemory();
double distance(const ob::State *p1, const ob::State *p2)
{
//define p1's and p2's StateType as SE3StateSpace
const ob::SE3StateSpace::StateType* state3Dp1 = p1->as<ob::SE3StateSpace::StateType>();
double x1 = state3Dp1->getX();
double y1 = state3Dp1->getY();
double z1 = state3Dp1->getZ();
const ob::SE3StateSpace::StateType* state3Dp2 = p2->as<ob::SE3StateSpace::StateType>();
double x2 = state3Dp2->getX();
double y2 = state3Dp2->getY();
double z2 = state3Dp2->getZ();
double dx = x1 - x2;
double dy = y1 - y2;
double dz = z1 - z2;
return sqrt(dx*dx + dy*dy + dz*dz);
}
double distanceFunction(const Motion *a, const Motion *b) const
{
ob::State *stateA = a->state;
ob::State *stateB = b->state;
ob::SE3StateSpace::StateType* stateTypeA = stateA->as<ob::SE3StateSpace::StateType>();
ob::SE3StateSpace::StateType* stateTypeB = stateB->as<ob::SE3StateSpace::StateType>();
double x1 = stateTypeA->getX();
double y1 = stateTypeA->getY();
double z1 = stateTypeA->getZ();
double x2 = stateTypeB->getX();
double y2 = stateTypeB->getY();
double z2 = stateTypeB->getZ();
double dx = x1 - x2;
double dy = y1 - y2;
double dz = z1 - z2;
return sqrt(dx * dx + dy * dy + dz * dz);
}
void interpolate(const ob::State *from, const ob::State *to, const double t, ob::State *state) const
{
const ob::SE3StateSpace::StateType* s1 = from->as<ob::SE3StateSpace::StateType>();
const ob::SE3StateSpace::StateType* s2 = to->as<ob::SE3StateSpace::StateType>();
ob::SE3StateSpace::StateType* lastValid = state->as<ob::SE3StateSpace::StateType>();
//Separate the path from s1 to s2 into nd Segment. nd = stateSpace_->validSegmentCount(s1, s2); in checkMotion and get the distance difference of X,Y,Z Value form s1 to s2
double distanceX = s2->getX() - s1->getX();
double distanceY = s2->getY() - s1->getY();
double distanceZ = s2->getZ() - s1->getZ();
lastValid->setXYZ(s1->getX() + distanceX*t, s1->getY() + distanceY*t, s1->getZ() + distanceZ*t);
}
//brief Grow a tree towards a random state
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion);
//brief State sampler
ompl::base::StateSamplerPtr sampler_;
//brief The start tree
TreeData tStart_;
//brief The goal tree
TreeData tGoal_;
//brief The maximum length of a motion to be added to a tree
double maxDistance_;
//brief The random number generator
ompl::RNG rng_;
/** \brief The pair of states in each tree connected during planning. Used for PlannerData computation */
std::pair<ompl::base::State*, ompl::base::State*> connectionPoint_;
};
}
名称空间ob=ompl:：base；
名称空间og=ompl:：geometric；
命名空间PathPlannerOMPLServer
{
类kdRRTConnect:public ompl:：base:：Planner
{
公众：
//简要构造
kdRRTConnect（常数ompl:：base:：SpaceInformationPtr&si）；
kdRRTConnect（）；
虚拟~kdRRTConnect（）；
虚拟void getPlannerData（ompl:：base:：PlannerData&data）常量；
虚拟ompl:：base:：Planner状态求解（常量ompl:：base:：Planner终止条件&ptc）；
虚空清除（）；
无效设置范围（双倍距离）
{
最大距离=距离；
}
双getRange（）常量
{
返回最大距离；
}
模板
void setnearestneights（）
{
t开始重置（新NN（））；
tStart->setDistanceFunction（std:：bind（&kdRRTConnect:：distanceFunction，this，std:：占位符：：_1，std:：占位符：：_2））；
重置（新的NN（））；
tGoal->setDistanceFunction（std:：bind（&kdRRTConnect:：distanceFun
namespace ob = ompl::base;
PathPlannerOMPLServer::kdRRTConnect::kdRRTConnect(const ompl::base::SpaceInformationPtr &si) : ompl::base::Planner(si, "kdRRTConnect")
{
specs_.recognizedGoal = ompl::base::GOAL_SAMPLEABLE_REGION;
specs_.directed = true;
maxDistance_ = 0.0;
Planner::declareParam<double>("range", this, &kdRRTConnect::setRange, &kdRRTConnect::getRange, "0.:1.:10000.");
connectionPoint_ = std::make_pair<ompl::base::State*, ompl::base::State*>(nullptr, nullptr);
}
PathPlannerOMPLServer::kdRRTConnect::kdRRTConnect()
:
ompl::base::Planner((const ompl::base::SpaceInformationPtr &)*((ompl::base::SpaceInformationPtr*)(unsigned char*)0), "kdRRTConnect")
{
//typedef std::shared_ptr <Motion> motion_ptr;
typedef std::shared_ptr < ompl::NearestNeighbors<Motion*>> nn_ptr;
nn_ptr nn_;
nn_->setDistanceFunction(std::bind(&kdRRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
PathPlannerOMPLServer::kdRRTConnect::~kdRRTConnect()
{
freeMemory();
}
void PathPlannerOMPLServer::kdRRTConnect::setup()
{
Planner::setup();
ompl::tools::SelfConfig sc(si_, getName());
sc.configurePlannerRange(maxDistance_);
if (!tStart_)
tStart_.reset(ompl::tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
if (!tGoal_)
tGoal_.reset(ompl::tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
tStart_->setDistanceFunction(std::bind(&kdRRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
tGoal_->setDistanceFunction(std::bind(&kdRRTConnect::distanceFunction, this, std::placeholders::_1, std::placeholders::_2));
}
void PathPlannerOMPLServer::kdRRTConnect::freeMemory()
{
std::vector<Motion*> motions;
if (tStart_)
{
tStart_->list(motions);
for (unsigned int i = 0; i < motions.size(); ++i)
{
if (motions[i]->state)
si_->freeState(motions[i]->state);
delete motions[i];
}
}
if (tGoal_)
{
tGoal_->list(motions);
for (unsigned int i = 0; i < motions.size(); ++i)
{
if (motions[i]->state)
si_->freeState(motions[i]->state);
delete motions[i];
}
}
}
void PathPlannerOMPLServer::kdRRTConnect::clear()
{
ompl::base::Planner::clear();
sampler_.reset();
freeMemory();
if (tStart_)
tStart_->clear();
if (tGoal_)
tGoal_->clear();
connectionPoint_ = std::make_pair<ompl::base::State*, ompl::base::State*>(nullptr, nullptr);
}
PathPlannerOMPLServer::kdRRTConnect::GrowState PathPlannerOMPLServer::kdRRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
{
/* find closest state in the tree */
Motion *nmotion = tree->nearest(rmotion);
/* assume we can reach the state we go towards */
bool reach = true;
/* find state to add */
ompl::base::State *dstate = rmotion->state;
double d = distance(nmotion->state, rmotion->state);
if (d > maxDistance_)
{
//si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
dstate = tgi.xstate;
reach = false;
}
// if we are in the start tree, we just check the motion like we normally do;
// if we are in the goal tree, we need to check the motion in reverse, but checkMotion() assumes the first state it receives as argument is valid,
// so we check that one first
bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) : si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
if (validMotion)
{
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
tree->add(motion);
if (reach)
return REACHED;
else
return ADVANCED;
}
else
return TRAPPED;
}
ompl::base::PlannerStatus PathPlannerOMPLServer::kdRRTConnect::solve(const ompl::base::PlannerTerminationCondition &ptc)
{
checkValidity();
ompl::base::GoalSampleableRegion *goal = dynamic_cast<ompl::base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return ompl::base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
while (const ompl::base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tStart_->add(motion);
}
if (tStart_->size() == 0)
{
OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
return ompl::base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return ompl::base::PlannerStatus::INVALID_GOAL;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(tStart_->size() + tGoal_->size()));
TreeGrowingInfo tgi;
tgi.xstate = si_->allocState();
Motion *rmotion = new Motion(si_);
ompl::base::State *rstate = rmotion->state;
bool startTree = true;
bool solved = false;
while (ptc == false)
{
TreeData &tree = startTree ? tStart_ : tGoal_;
tgi.start = startTree;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
const ompl::base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tGoal_->add(motion);
}
if (tGoal_->size() == 0)
{
OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
break;
}
}
/* sample random state */
sampler_->sampleUniform(rstate);
GrowState gs = growTree(tree, tgi, rmotion);
if (gs != TRAPPED)
{
/* remember which motion was just added */
Motion *addedMotion = tgi.xmotion;
/* attempt to connect trees */
/* if reached, it means we used rstate directly, no need top copy again */
if (gs != REACHED)
si_->copyState(rstate, tgi.xstate);
GrowState gsc = ADVANCED;
tgi.start = startTree;
while (gsc == ADVANCED)
gsc = growTree(otherTree, tgi, rmotion);
Motion *startMotion = startTree ? tgi.xmotion : addedMotion;
Motion *goalMotion = startTree ? addedMotion : tgi.xmotion;
/* if we connected the trees in a valid way (start and goal pair is valid)*/
if (gsc == REACHED && goal->isStartGoalPairValid(startMotion->root, goalMotion->root))
{
// it must be the case that either the start tree or the goal tree has made some progress
// so one of the parents is not nullptr. We go one step 'back' to avoid having a duplicate state
// on the solution path
if (startMotion->parent)
startMotion = startMotion->parent;
else
goalMotion = goalMotion->parent;
connectionPoint_ = std::make_pair(startMotion->state, goalMotion->state);
/* construct the solution path */
Motion *solution = startMotion;
std::vector<Motion*> mpath1;
while (solution != nullptr)
{
mpath1.push_back(solution);
solution = solution->parent;
}
solution = goalMotion;
std::vector<Motion*> mpath2;
while (solution != nullptr)
{
mpath2.push_back(solution);
solution = solution->parent;
}
ompl::geometric::PathGeometric *path = new ompl::geometric::PathGeometric(si_);
path->getStates().reserve(mpath1.size() + mpath2.size());
for (int i = mpath1.size() - 1; i >= 0; --i)
path->append(mpath1[i]->state);
for (unsigned int i = 0; i < mpath2.size(); ++i)
path->append(mpath2[i]->state);
pdef_->addSolutionPath(ompl::base::PathPtr(path), false, 0.0, getName());
solved = true;
break;
}
}
}
si_->freeState(tgi.xstate);
si_->freeState(rstate);
delete rmotion;
OMPL_INFORM("%s: Created %u states (%u start + %u goal)", getName().c_str(), tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
return solved ? ompl::base::PlannerStatus::EXACT_SOLUTION : ompl::base::PlannerStatus::TIMEOUT;
}
void PathPlannerOMPLServer::kdRRTConnect::getPlannerData(ompl::base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (tStart_)
tStart_->list(motions);
for (unsigned int i = 0; i < motions.size(); ++i)
{
if (motions[i]->parent == nullptr)
data.addStartVertex(ompl::base::PlannerDataVertex(motions[i]->state, 1));
else
{
data.addEdge(ompl::base::PlannerDataVertex(motions[i]->parent->state, 1),
ompl::base::PlannerDataVertex(motions[i]->state, 1));
}
}
motions.clear();
if (tGoal_)
tGoal_->list(motions);
for (unsigned int i = 0; i < motions.size(); ++i)
{
if (motions[i]->parent == nullptr)
data.addGoalVertex(ompl::base::PlannerDataVertex(motions[i]->state, 2));
else
{
// The edges in the goal tree are reversed to be consistent with start tree
data.addEdge(ompl::base::PlannerDataVertex(motions[i]->state, 2),
ompl::base::PlannerDataVertex(motions[i]->parent->state, 2));
}
}
// Add the edge connecting the two trees
data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
}