- solve总逻辑。进行一轮轮迭代，在每轮迭代，1）生成一个随机结点rmotion。2）调用growTree，以rmotion为新采样点，对初始树进行扩展。3）确保rmotion结点值等于新添加到初始树的结点值。3）不断扩展目标树，直到或扩展到了rmotion，意味着找到路径，解决；或遇到了碰撞冲突、无法再扩展，进入下一轮迭代。
- 系统存在两棵树：tStart_、tGoal_。在每轮迭代，是tStart_还是tGoal_作为初始树，是交替的。换句话说，第一轮时tStart_是初始树，tGoal_是目标树。到第二轮，换作tGoal_是初始树，tStart_是目标树。第三轮和第一轮一样，依此类推。
- 在每一轮迭代，初始树只会扩展一次，目标树可能多次。
- rmotion这个结点的root、parent总是nullptr，和它同值(state字段)的结点会存在树中，但它不会存在任何树。
- RRTConnect::solve后得到的path_不是client希望的规划轨迹，后续需要依次调用simplifySolution、interpolateSolution进行转换。
一、一个运行示例
- 机械臂有a、b、c三个关节，都是单自由度。
- 初始状态：(-0.56, 0.79, -0.45)
- 目标状态：(1, 2, -1)
以下是示例的执行过程。
- 初始结点(-0.56, 0.79, -0.45)加入初始树，树中结点数1。进入第1轮迭代。
- sampleUniform随机生成一个采样值，值更新到rmotion(-0.87008, 2.72230, 0.23816)。
- [第一次growTree]nearest(rmotion)找到离rmotion最近的树中结点(-0.56, 0.79, -0.45)，记为nmotion。
- [growTree]算出nmotion和rmotion两结点距离是2.93054，大于扩展阀值1.884。于是以nmotion为起点，最大可扩展距离1.884为增加值，生成新采样点(-0.75934, 2.03224, -0.00759)，记为dstate。
- [growTree]checkMotion(dstate)进行碰撞检测，结果是没有发生碰撞。（注：如果发生碰撞，growTree返回TRAPPED）。
- [growTree]以dstate=>motion->state、nmotion=>param，nmotion->root=>motion->root，把这个新生成的motion结点加入扩展树，树中结点数2。随即growTree返回ADVANCED，表示此次growTree在随机采样状态方面取得了进展，这个进展应该就是的加入了一个新结点motion。而且tgi.xmotion指向这个新加入的结点。
- 因为growTree不是返回TRAPPED，进入目标树环节。由于growTree不是返回REACHED，更新上一次growTree内新产生的采样点到rstate，即rstate值是(-0.75934, 2.03224, -0.00759)。
- [第二次growTree]nearest(rmotion)找到离rmotion最近的目标树中结点(1, 2, -1)，记为nmotion。
- [growTree]算出nmotion和rmotion两结点距离是2.78395，大于扩展阀值1.884。于是以nmotion为起点，最大可扩展距离1.884为增加值，生成新采样点(-0.19061, 2.02185, -0.32842)，记为dstate。
- [growTree]checkMotion(dstate)进行碰撞检测，结果是没有发生碰撞。
- [growTree]以dstate=>motion->state、nmotion=>param，nmotion->root=>motion->root，把这个新生成的motion结点加入扩展树，树中结点数2。随即growTree返回ADVANCED，表示此次growTree在随机采样状态方面取得了进展，这个进展应该就是的加入了一个新结点motion。而且tgi.xmotion指向这个新加入的结点。
- 因为growTree返回ADVANCED，继续在目标树调用growTree。此时rmotion依旧是(-0.75934, 2.03224, -0.00759)，不是最近一次的(-0.19061, 2.02185, -0.32842)。
- [第三次growTree]nearest(rmotion)找到离rmotion最近的目标树中结点(-0.19061, 2.02185, -0.32842)，记为nmotion。
- [growTree]算出nmotion和rmotion两结点距离是0.89995，<=扩展阀值1.884。于是保持dstate值是rmotion、reach=true。
- [growTree]checkMotion(dstate)进行碰撞检测，结果是没有发生碰撞。
- [growTree]以dstate=>motion->state、nmotion=>param，nmotion->root=>motion->root，把这个新生成的motion结点加入目标树，树中结点数3。随即growTree返回REACHED，表示此次growTree达到随机采样rmotion，而且tgi.xmotion指向这个rmotion。注：rmotion结点已经在起点树，但这里操作的是目标树，这里的增加是加到目标树。
- 由于growTree返回REACHED，退出while (gsc == ADVANCED)，继续solve后续逻辑。
- startMotion: (-0.75934, 2.03224, -0.00759), goalMotion: (-0.75934, 2.03224, -0.00759)
- 为避免生成的路径中含有重复结点，要让startMotion或goalMotion当中一个往后退一代。首选是先退startMotion。两棵树中，根结点的parent都是nullptr。
- 根据初始树生成路径mpath1。生成方法是从startMotion开始，通过parent向上溯。由于startMotion就是初始结点(-0.56, 0.79, -0.45)，mpath1长度是1。
- 根据目标树生成路径mpath2。生成方法是从goalMotionMotion开始，通过parent向上溯。goalMotionMotion就是起始结点(-0.75934, 2.03224, -0.00759)，mpath2长度是3。
- 合并path1、path2，结果放在path_的states_。states_长度是4，第一个是起始点，最后一个是目标点。
[ INFO] [1648085719.153534100]: robot_arm/robot_arm: Created 5 states (2 start + 3 goal)。——总共创建了5个结点，初始树中2个，目标树中3个。
图2显示了RRTConnect::solver后生成的路径，有4个点，就是图1从初始树的初始结点，一到目标树的目标结点。
PathGeometric::length()返回的不是states_长度，而是各点之间的距离和(distance)。
二、RRTConnect::growTree
新来了一个采样点rmotion，看如何加入随机树tree。如果肯定能通过冲突检测，怎么加入分两种情况：1）采样点位在可扩展半径内，该采样点加入随机树。2）采样点落在可扩展半径外，在nmotion和rmotion之间新算出一个采样点，把该采样点加入随机树。
@tree：要生长的树。可能是初始树，可能是目标树，可根据tgi.start判断是哪种树。
@tgi：struct TreeGrowingInfo。用于在计算过程中存储输助信息。1）growTree时，光一个GrowState返回值是不够的，它的xstate、xmotion也存储着“返回值”。2）start字段指示了参数tree是tStart_还是tGoal_。
- base::State *xstate。如果此次growTree发生了interpolate，它存储着interpolate出的结点值。
- Motion *xmotion。此个growTree向随机树新添加的结点。
- bool start。true表示此次处理随机树tree是tStart_，否则处理的是tGoal_。
@rmotion：class Motion。一个Motion表示一个树中结点。此处表示一个新采样出的结点，该结点肯定没在tree中。
- const base::State *root。该结点从此root所在结点扩展出来。以它为根，会有一条枝桠，枝桠上所有结点有着同一个root。对初始树，它就是初始结点，对目标树，它就是目标结点。在图1目标树中的三个结点，它们root都是(1, 2, -1)。
- base::State *state。结点值。
- Motion *parent。该结点的直系父结点。初始树中初始结点、目标树中目标结点的parent是nullptr，其它结点都是非nullptr。在图1中目标树中，(-0.75934, 2.03224, -0.00759)的parent是(-0.19061, 2.02185, -0.32842)。
返回值：enum GrowState。
- TRAPPED。在新采样会发生碰撞，没有取得任何进展。
- ADVANCED。在随机采样状态方面取得了进展。向随机树添清加一个结点，但添加的不是希望的rmotion。
- REACHED。成功扩展到随机采样点rmotion。向随机树添清加rmotion结点。
ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion)
{
/* find closest state in the tree */
Motion *nmotion = tree->nearest(rmotion);找出随机树中离rmotion最近结点，新指针nmotion指向树上这个结点。
/* assume we can reach the state we go towards */ bool reach = true; /* find state to add */ base::State *dstate = rmotion->state; double d = si_->distance(nmotion->state, rmotion->state);
计算nmotion、rmotion之间距离。计算距离时要用到机器人模型。
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);距离大于一次可扩展的最大距离，则插入中间点。中间点是以nmotion为起点，朝向rmotion扩展出maxDistance_距离。注意，tgi.xstate存储了此个中间点的值。
/* Check if we have moved at all. Due to some stranger state spaces (e.g., the constrained state spaces), * interpolate can fail and no progress is made. Without this check, the algorithm gets stuck in a loop as it * thinks it is making progress, when none is actually occurring. */ if (si_->equalStates(nmotion->state, tgi.xstate)) return TRAPPED;
si_->equalStates逻辑基本可认为就是判断nmotion->state、tgi.xstate数值上是否相等。
dstate = tgi.xstate; reach = false; } bool validMotion = tgi.start ? si_->checkMotion(nmotion->state, dstate) : si_->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
checkMotion执行冲突检测。
if (!validMotion)
return TRAPPED;
if (addIntermediateStates_)
{
// addIntermediateStates_默认false，不进这入口
}
else
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tree->add(motion);
tgi.xmotion = motion;向随机树添加一个结点。reach=true时，这个结点就是rmotion，否则是interpolate。注意，tgi.xmotion指向这个新加入的结点。
} return reach ? REACHED : ADVANCED; }
三、RevoluteJointModel::interpolate
growTree计算nmotion和rmotion距离，当距离大于一次可扩展的最大距离，要插入中间点。中间点是以nmotion为起点，朝向rmotion扩展出maxDistance_距离。生成中间点，说来就是要生成这State表示的m维值。针对示例，有三个单自由度关节，这个State的是个3维值。生成一个维度值方法是调用一次RevoluteJointModel::interpolate，3维意味着要调用三次。
- @from: 起始值。对示例中的从nmotion(-0.56, 0.79, -0.45)到rmotion(-0.87008, 2.72230, 0.23816)，对a关节，from是-0.56。
- @to: 终点值。对@from示例，对a关节，to是-0.87008。
- @t: 范围(0, 1)double系数。对@from示例，值来自公式“maxDistance_ / d”，即“1.884 / 2.93054”。
void RevoluteJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
{
if (continuous_)
{continuous_：该关节是否做圆周运动。true表示做圆周运动。
... } else state[0] = from[0] + (to[0] - from[0]) * t; }
后面“ompl生成路径[2/2]：simplifySolution、interpolateSolution”，调用pg.interpolate补插path_时，也是调用RevoluteJointModel::interpolate，这些新插入的平均分布在该段。
四、RRTConnect::solve
ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
if (goal == nullptr)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tStart_->add(motion);把初始结点添加到tStart_树，这些结点parent是nullptr。对示例，只一个初始结点。
}
if (tStart_->size() == 0)
{
OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return 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 *approxsol = nullptr;
double approxdif = std::numeric_limits<double>::infinity();
auto *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;构造一个随机结点rmotion。在“while (!ptr)”，该结点将一直存在，但会有两次更新值。1)更新到一个sampleUniform随机值。2)如果在扩展初始树时添加的是interpolate结点，更新到interpolate值。不论何时，rmotion的parent、root一直是nullptr。
bool solved = false;
while (!ptc)
{
TreeData &tree = startTree_ ? tStart_ : tGoal_;
tgi.start = startTree_;
startTree_ = !startTree_;
TreeData &otherTree = startTree_ ? tStart_ : tGoal_;进入第N轮迭代。在每轮迭代，是tStart_还是tGoal_作为初始树，由bool变量startTree_决定，是交替的。换句话说，第一轮时tStart_是初始树tree，tGoal_是目标树otherTree。到第二轮，换作tGoal_是初始树tree_，tStart_是目标树otherTree。第三轮和第一轮一样，依此类推。
if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
{
const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st != nullptr)
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->root = motion->state;
tGoal_->add(motion);把目标结点添加到tGoal_树，这些结点parent是nullptr。对示例，只一个目标结点。
}
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);随机产生一个采样点，值存在rstate。由于rstate是所有点是结点rmotion，于是rmotion同时更新到这个采样值。接下就是调用growTree，以rmotion为新采样点，在对初始树进行扩展。
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 to copy again */
if (gs != REACHED)
si_->copyState(rstate, tgi.xstate);进这个入口，意味着gs返回值或是REACHED，或是ADVANCED。不论哪种，初始树是肯定加了一个结点，而tgi.xmotion指向那个新添加初始树的结点。
gs != REACHED，意味着向初始树添加的是interpolate结点，此时tgi.xstate存储着这个interpolate结点值，copyState把这值更新到rmotion。也就是说，到此刻rmotion的结点值(state字段)肯定等于addedMotion，但root、parent可能不一样，rmotion的这两个字段总是nullptr。
tgi.start = startTree_; /* if initial progress cannot be done from the otherTree, restore tgi.start */ GrowState gsc = growTree(otherTree, tgi, rmotion); if (gsc == TRAPPED) tgi.start = !tgi.start; while (gsc == ADVANCED) gsc = growTree(otherTree, tgi, rmotion);
不断扩展目标树，直到或扩展到了rmotion，或遇到了碰撞冲突、无法再扩展。
/* update distance between trees */
const double newDist = tree->getDistanceFunction()(addedMotion, otherTree->nearest(addedMotion));
if (newDist < distanceBetweenTrees_)
{
distanceBetweenTrees_ = newDist;
// OMPL_INFORM("Estimated distance to go: %f", distanceBetweenTrees_);
}
Motion *startMotion = tgi.start ? tgi.xmotion : addedMotion;
Motion *goalMotion = tgi.start ? addedMotion : tgi.xmotion;tig.xmotion是最近一次添加到目标树结点，如果目标树成功扩展到rmotion(gsc == REACHED)，意味着tgi.xmotion的state值等于addedMotion，但它们一个属于目标树，一个属于初始树，root、parent都不一样。针对图1，startMotion、goalMotion就是两棵树都共同存在的结点值(-0.75934, 2.03224, -0.00759)，图中归为一个，实际是互不关联的两个。startMotion指向初始树这个，goalMotion指向目标树那个。
/* 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 != nullptr)
startMotion = startMotion->parent;
else
goalMotion = goalMotion->parent;startMotion和goalMotion虽是两个结点，但值是相同的，为避免生成的路径中含有相同值的结点，要让startMotion或goalMotion当中一个往后退一代。优先是先退startMotion。两棵树中，有且只有根结点的parent都是nullptr。
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;
}根据初始树生成路径mpath1。生成方法是从startMotion开始，通过parent向上溯。
solution = goalMotion;
std::vector<Motion *> mpath2;
while (solution != nullptr)
{
mpath2.push_back(solution);
solution = solution->parent;
}根据目标树生成路径mpath2。生成方法是从goalMotionMotion开始，通过parent向上溯。
auto path(std::make_shared<PathGeometric>(si_)); path->getStates().reserve(mpath1.size() + mpath2.size()); for (int i = mpath1.size() - 1; i >= 0; --i) path->append(mpath1[i]->state); for (auto &i : mpath2) path->append(i->state); pdef_->addSolutionPath(path, false, 0.0, getName());
合并path1、path2，结果放在path_的states_。states_第一个是起始点，最后一个是目标点。
solved = true;
break;
}
else
{
// We didn't reach the goal, but if we were extending the start
// tree, then we can mark/improve the approximate path so far.
if (tgi.start)
{
// We were working from the startTree.
double dist = 0.0;
goal->isSatisfied(tgi.xmotion->state, &dist);
if (dist < approxdif)
{
approxdif = dist;
approxsol = tgi.xmotion;
}
}
}
}
}
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());
if (approxsol && !solved)
{
...
}
return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}