目标状态器是从Goal派生的类对象，派生类较多，较常见的使用类是ConstrainedGoalSampler。
之前关于规划文章可知，client发来的moveit_msgs::Constraints类型表示的目标要求，最终要处理成目标结点，并添加到目标树tGoal。本文描述内中过程，核心是要用到一个目标状态器ConstrainedGoalSampler。
- ActionServerThread线程。从client发来的任务中提取出以moveit_msgs::Constraints类型表示的目标要求，处理成ConstraintSampler类型，值存储在constraint_sampler_成员。
- 启动goalSamplingThread线程。开始第一轮，每轮先调用samplerFunc_去采样状态，samplerFunc_是函数指针，指向ConstrainedGoalSampler::sampleUsingConstraintSampler。该函数会调用ConstrainedGoalSampler的sample()方法，采样出一个State，存储在临时变量s。然后调用addStateIfDifferent，把s添加到集合states_。此轮结束后，线程继续下一轮的sample()、存储到s、添加到states_。
- 在规划线程的RRTConnect::solve(...)，调用PlannerInputStates的nextGoal()，nextGoal则调用GoalSampleableRegion（ConstrainedGoalSampler基类）的sampleGoal(...)读出states_中的一个State，存储在变量tempState_。继续把tempState_返回到solve，solve用它生成一个目标结点，加入目标树tGoal_。
接下深入这三个步骤，但在说它们前，了解下Goal的众多派生类。
一、Goal以及派生类
ompl::base::Goal是目标状态器基类，Goal有着多级派生类。
Goal有个枚举类型的成员type_，它指示了对象是哪个派生类。
| 类 | type_ | 描述 |
| Goal | GOAL_ANY = 1 | |
| GoalRegion | GOAL_REGION = GOAL_ANY + 2 | 这个目标状态器可计算出指定状态(state)和目标间距离，而且该状态在目标区域内的条件是这个距离小于规定的阈值。 |
| GoalSampleableRegion | GOAL_SAMPLEABLE_REGION = | 可在目标区域进行采样 |
| GoalStates | GOAL_STATES = | |
| GoalLazySamples | GOAL_LAZY_SAMPLES = | 需预定义一个可以采样的目标区域，但采样过程可能很慢。它会在单独线程中进行采样，并且随着规划持续运行，目标数可能会增加，当然增加时会保证线程安全。 |
| ConstrainedGoalSampler | 等于GoalLazySamples |
给type_赋值时循寻一条规则：N级派生类是在N-1级type_上再加一个位掩码。也就是说，按“&”位掩码去算的话，如果N-1返回true，那N级去“&”同一个值时，一定返回true。举个例子，GoalSampleableRegion的type_含有GOAL_SAMPLEABLE_REGION，那它的派生类GoalStates、GoalLazySamples、ConstrainedGoalSampler去“& GOAL_SAMPLEABLE_REGION”时，一定返回true。
随着派生代数增加，派生类会不断增多(纯)虚函数。基类Goal就一个纯虚函数：ompl::base::Goal::isSatisfied(const State *st)，用来判断输入的状态st是否为目标。
class GoalSampleableRegion : public GoalRegion
{
/** \brief Return the maximum number of samples that can be asked for before repeating */
// 返回重复前可以请求的最大样本数
virtual unsigned int maxSampleCount() const = 0;
/** \brief Return true if maxSampleCount() > 0, since in this case samples can certainly be produced */
// 如果 maxSampleCount() > 0，则返回 true，因为在这种情况下，样本肯定可以生成
bool canSample() const
{
return maxSampleCount() > 0;
}
};派生类多出的(纯)虚函数和它增加的功能有关。GoalSampleableRegion这种目标状态器声明了它可在目标区域进行采样，于是增加了一个表示目前已有多少个采样点的maxSampleCount()。canSample()不是虚函数，只要内中存有了采样点，它就返回true。
class GoalStates : public GoalSampleableRegion
{
void sampleGoal(State *st) const override;
virtual void addState(const State *st);
protected:
/** \brief The goal states. Only ones that are valid are considered by the motion planner. */
// 目标状态。只有有效的运动规划器才会考虑这些。
std::vector<State *> states_;
};
void ompl::base::GoalStates::sampleGoal(base::State *st) const
{
if (states_.empty())
throw Exception("There are no goals to sample");
// Roll over the samplePosition_ if it points past the number of states.
samplePosition_ = samplePosition_ % states_.size();
// Get the next state.
si_->copyState(st, states_[samplePosition_]);在状态器内部，维护着一个读索引samplePosition_，如果上次读出状态数组最后一个，此次读索引回到0。为避回0读出旧状态，调用者往往出会自已维护个读索引，像PlannerInputStates中的sampledGoalsCount_。
// Increment the counter. Do NOT roll over incase a new state is added before sampleGoal is called again. samplePosition_++; }
相比GoalSampleableRegion，GoalStates更进一步明确存储采样点的据结构是std::vector，添加采样点的方法是addState。也就是说，如果你的目标器可在目标区域进行采样，而且可以以std::vector数据结构存储，为简洁，那优先使用GoalStates。
二、ActionServerThread线程：生成constraint_sampler_，用它创建目标状态器ConstrainedGoalSampler
看这节时要对照“ompl规划[1/5]：关节规划”中“一、设置目标状态”。
要生成的constraint_sampler_封装了目标值，类型是constraint_samplers::ConstraintSampler的一种派生类，按不同规划场景，派生类有两种。
- 关节规划：constraint_samplers::JointConstraintSampler(关节受限的采样器)
- 位姿规划：constraint_samplers::IKConstraintSampler(位姿受限的采样器)
重复抄下如何创建状态目标器ConstrainedGoalSampler。
ompl::base::GoalPtr ompl_interface::ModelBasedPlanningContext::constructGoal()
{
// ******************* set up the goal representation, based on goal constraints
std::vector<ob::GoalPtr> goals;
for (kinematic_constraints::KinematicConstraintSetPtr& goal_constraint : goal_constraints_)
{
constraint_samplers::ConstraintSamplerPtr constraint_sampler;
if (spec_.constraint_sampler_manager_)
constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(),
goal_constraint->getAllConstraints());
if (constraint_sampler)
{
ob::GoalPtr goal = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraint, constraint_sampler));此阶段要生成目标状态器ConstrainedGoalSampler，要三个参数，前两个可说是现成的，需要即时生成的只有ConstraintSampler类型的constraint_sampler，这个cs赋值给ConstrainedGoalSampler的成员变量constraint_sampler_。ConstrainedGoalSampler内中成员见“ompl规划[1/5]：关节规划”图1。
goals.push_back(goal); } } ... }
constructGoal()如何得到constraint_sampler？——来自spec_.constraint_sampler_manager_->selectSampler(...)。
constraint_samplers::ConstraintSamplerPtr
constraint_samplers::ConstraintSamplerManager::selectSampler(const planning_scene::PlanningSceneConstPtr& scene,
const std::string& group_name,
const moveit_msgs::Constraints& constr) const
{
for (const ConstraintSamplerAllocatorPtr& sampler : sampler_alloc_)
if (sampler->canService(scene, group_name, constr))
return sampler->alloc(scene, group_name, constr);
sampler_alloc_存储着预定义分配器，一般来说不会有预定义，即sampler_alloc_是空。
// if no default sampler was used, try a default one return selectDefaultSampler(scene, group_name, constr); }
没有预定义分配器时，直接调用selectDefaultSampler。这函数较复杂，这里只抄和关节规划相关的。
constraint_samplers::ConstraintSamplerPtr
constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planning_scene::PlanningSceneConstPtr& scene,
const std::string& group_name,
const moveit_msgs::Constraints& constr)
{
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name);
if (!jmg)
return constraint_samplers::ConstraintSamplerPtr();
...
ConstraintSamplerPtr joint_sampler; // location to put chosen joint sampler if needed
// if there are joint constraints, we could possibly get a sampler from those
if (!constr.joint_constraints.empty())
{参数constr类型是moveit_msgs::Constraints，在关节规划场景，constr中存储约束的字段中只有joint_constraints是非空。顺便说下，位姿规划时，变成position_constraints、orientation_constraints同时非空。
ROS_DEBUG_NAMED("constraint_samplers",
"There are joint constraints specified. "
"Attempting to construct a JointConstraintSampler for group '%s'",
jmg->getName().c_str());
std::map<std::string, bool> joint_coverage;
for (const std::string& joint : jmg->getVariableNames())
joint_coverage[joint] = false;
// construct the constraints
std::vector<kinematic_constraints::JointConstraint> jc;
for (const moveit_msgs::JointConstraint& joint_constraint : constr.joint_constraints)
{
kinematic_constraints::JointConstraint j(scene->getRobotModel());
if (j.configure(joint_constraint))
{
if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
{
joint_coverage[j.getJointVariableName()] = true;
jc.push_back(j);
}
}
}
// check if every joint is covered (constrained) by just joint samplers
bool full_coverage = true;
for (const std::pair<const std::string, bool>& it : joint_coverage)
if (!it.second)
{
full_coverage = false;
break;
}变量joint_coverage类型std::map，first是关节易记名，second则表示了urdf这否定义了该关节，一次实时值见图3。示例中所有关节都在urdf中定义了，于是full_coverage = true。
// if we have constrained every joint, then we just use a sampler using these constraints
if (full_coverage)
{
JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
if (sampler->configure(jc))
{
ROS_DEBUG_NAMED("constraint_samplers", "Allocated a sampler satisfying joint constraints for group '%s'",
jmg->getName().c_str());
return sampler;
关节规划场景在这个入口返回。返回的sampler类型是JointConstraintSampler。图3左侧显示了输入constr，右侧显示了输出sampler。sampler.bounds_存储着转换后的目标js，说来就是把原js加上tolerance。对右侧sampler中的values_，它此时不是有有效值，接下的JointConstraintSampler::sample会给它赋上最近一次的采样值。
} } ... } ... }
selectDefaultSampler(...)创建哪种ConstraintSampler依赖于参数constr的实时值。向上溯源，可知constr来自goal_constraints_[n].all_constraints_，计算它则在setGoalConstraints。
bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints(
const std::vector<moveit_msgs::Constraints>& goal_constraints, const moveit_msgs::Constraints& path_constraints,
moveit_msgs::MoveItErrorCodes* error)
{
// ******************* check if the input is correct
goal_constraints_.clear();
for (const moveit_msgs::Constraints& goal_constraint : goal_constraints)
{
moveit_msgs::Constraints constr = kinematic_constraints::mergeConstraints(goal_constraint, path_constraints);此处算出的constr就是传给selectDefaultSampler(...)的constr。通常参数path_constraints是空，此时值全来自于goal_constraint，它就是client传过来的req.goal_constraints。
kinematic_constraints::KinematicConstraintSetPtr kset( new kinematic_constraints::KinematicConstraintSet(getRobotModel())); kset->add(constr, getPlanningScene()->getTransforms()); if (!kset->empty()) goal_constraints_.push_back(kset); } ... }
以下小结目标值依次转换到变量及类型
- req.goal_constraints[n]。类型：moveit_msgs::Constraints
- goal_constraints_[n].all_constraints_。类型：moveit_msgs::Constraints
- goals[n].constraint_sampler_。类型：constraint_samplers::ConstraintSampler
附上moveit_msgs::Constraints各成员概述。
moveit_msgs::Constraints std::string name: 空 std::vector<moveit_msgs::JointConstraint> joint_constraints。关节规划时不为0。长度是关节数。 std::vector<moveit_msgs::PositionConstraint> position_constraints。空间规划时为1，其它空。 std::vector<moveit_msgs::OrientationConstraint> orientation_constraints。空间规划时为1，其它空。 std::vector<moveit_msgs::VisibilityConstraint> visibility_constraints。空。
三、goalSamplingThread线程：采样出State，添加到States_
在GoalStates基础上，GoalLazySamples增加了如何采样，方法就是创建一个goalSamplingThread的线程，在该线程内进行采样。采样过程可能很慢，并且随着规划持续运行，目标数可能会增加，当然增加时会保证线程安全。
void ompl::base::GoalLazySamples::goalSamplingThread()
{
...
unsigned int prevsa = samplingAttempts_;
if (isSampling() && samplerFunc_)
{
OMPL_DEBUG("Beginning sampling thread computation");
ScopedState<> s(si_);
while (isSampling() && samplerFunc_(this, s.get()))
{isSampling()=true，表示调用者希望线程继续采集。samplerFunc_是个函数指针，当目标状态器是ConstrainedGoalSampler，实际指向的是ConstrainedGoalSampler::sampleUsingConstraintSampler。
++samplingAttempts_;
if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
{
OMPL_DEBUG("Adding goal state");
addStateIfDifferent(s.get(), minDist_);
}
else
{
OMPL_DEBUG("Invalid goal candidate");
}
}
}
...
}3.1 ConstrainedGoalSampler::sampleUsingConstraintSampler
要是成功，一次sampleUsingConstraintSampler会采出一个State，结果放在new_goal。
bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const ob::GoalLazySamples* gls,
ob::State* new_goal)
{
...
for (unsigned int a = gls->samplingAttemptsCount(); a < max_attempts && gls->isSampling(); ++a)
{
...
if (constraint_sampler_)
{constraint_sampler_存储着私有采样器，关节规划时JointConstraintSampler，位姿规划时IKConstraintSampler。
// makes the constraint sampler also perform a validity callback
moveit::core::GroupStateValidityCallbackFn gsvcf =
std::bind(&ompl_interface::ConstrainedGoalSampler::stateValidityCallback, this, new_goal,
std::placeholders::_1, // pointer to state
std::placeholders::_2, // const* joint model group
std::placeholders::_3, // double* of joint positions
verbose);
constraint_sampler_->setGroupStateValidityCallback(gsvcf);
if (constraint_sampler_->sample(work_state_, planning_context_->getMaximumStateSamplingAttempts()))
{调用采样器的sample方法，sample产生一个指定范围内的随机值，同时该值更新到work_state.position_。
work_state_.update();
if (kinematic_constraint_set_->decide(work_state_, verbose).satisfied)
{调用decide方法，它判断新采出的State是否满足约束。
if (checkStateValidity(new_goal, work_state_, verbose)) return true;
checkStateValidity把work_state_.position_状态值更新new_goal。综合以上步骤，总的是逻辑是这样的：
- 私有采样器constraint_sampler_调用sample(...)采样出一个状态，放在work_state.position_。
- checkStateValidity把work_state.position_更新到new_goal。于是sample(...)生成采样值就是要放入目标状态器的状态。
} ... } } ... } return false; }
3.2 JointConstraintSampler::sample
要是成功，一次sample会采出一个State，结果放在values_成员，同时更新到state.position_。
bool JointConstraintSampler::sample(moveit::core::RobotState& state,
const moveit::core::RobotState& /* reference_state */,
unsigned int /* max_attempts */)
{
...
// enforce the constraints for the constrained components (could be all of them)
for (const JointInfo& bound : bounds_)
values_[bound.index_] = random_number_generator_.uniformReal(bound.min_bound_, bound.max_bound_);
state.setJointGroupPositions(jmg_, values_);bounds_是什么值见图3中右侧的sampler。举个例子，bounds_[0]=(0.9999, 10001)，那uniformReal将产生一个(0.9999, 10001)范围内随机数。有多少个关节，就须要调用多少次uniformReal。
生成的state放在values_成员，同时更新到state.position_。
// we are always successful return true; }
至此除了state.position_外，sampler的values_也存储着最新采样值。
四、规划线程：nextGoal读出State，生成目标结点放入目标树
假设规划算法是RRTConnect，那规划主函数是RRTConnect::solve(...)。
ompl::base::PlannerStatus ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
{
...
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);
}
...
}为得到目标结点值，solve是通过pis_.nextGoal方法得到目标结点值，pis_类型是PlannerInputStates。得到值后生成目标结点，添加到目标树tGoal_。
4.1 PlannerInputStates::nextGoal
const ompl::base::State *ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
{
...
if (pdef_->getGoal() != nullptr)
{
const GoalSampleableRegion *goal =
pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;根据上面所述的type_赋值规则，type_含有GOAL_SAMPLEABLE_REGION，意味着pdef_->goal_至少是GoalSampleableRegion，知道这个就可调用getGoal()->as(...)，把goal安全转换到GoalSampleableRegion类型。以上这条语句有这么个结果：只要目标状态器pdef_->goal_至少是GoalSampleableRegion或从它派生，则goal!=nullptr。
if (goal != nullptr)
{
time::point start_wait;
bool first = true;
bool attempt = true;
while (attempt)
{
attempt = false;
if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
{goal->maxSampleCount()返回可以请求的最大样本数。goal->canSample()逻辑是“maxSampleCount()>0”，因而if条件其实只有前半部。
if (tempState_ == nullptr)
tempState_ = si_->allocState();
do
{
goal->sampleGoal(tempState_);从目标状态器读出一个目标，sampleGoal(...)是怎么读的见上面GoalStates注释。pis维护着一个读索引sampledGoalsCount_，如果它>=goal->maxSampleCount()，意味着已读过目标状态器中的所有states_。
sampledGoalsCount_++;
bool bounds = si_->satisfiesBounds(tempState_);
bool valid = bounds ? si_->isValid(tempState_) : false;
if (bounds && valid)
{
...
return tempState_;
}
...
} while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
}
...
}
}
}
return nullptr;
}