Setting the goal
The most general representation of a goal is ompl::base::Goal. This class contains a pure virtual method, ompl::base::Goal::isSatisfied(), which takes a state as argument and returns a boolean indicating whether that state is a goal or not. No other information about the goal is given. This function can include arbitrary code deciding what is in the goal region and what is not.
{
public:
MyArbitraryGoal(const SpaceInformationPtr &si) : ompl::base::Goal(si)
{
}
{
}
};
While this is a very general definition, it is often the case more information is known about the goal. This information is helpful for planners and the user should specify this information when available. Here are types of information planners can use:
- If an approximation of distance to the goal is available, not necessarily a metric, this information can be given to the planner by also providing an implementation of ompl::base::Goal::isSatisfied() (the version that takes a ompl::base::State* and a double* as arguments). By default, the implementation of this function simply sets the distance to the maximum value a double can hold.
{
public:
MyArbitraryGoal(const SpaceInformationPtr &si) : ompl::base::Goal(si)
{
}
{
perform_any_operations();
return truth value;
}
virtual bool isSatisfied(
const State *st,
double *distance)
const
{
if (distance != NULL)
{
if (state_clearly_out_of_goal_region(st))
{
*distance = std::numeric_limits<double>::max();
}
else
{
if (state_near_goal(st))
*distance = 1;
else
*distance = 100;
}
}
return result;
}
};
- It is often the case that when distance to goal approximations are available, the condition for a state being in the goal region is that that distance is less than a specified threshold. If this is indeed the case, ompl::base::GoalRegion (which inherits from ompl::base::Goal) should be used. The additional function ompl::base::GoalRegion::distanceGoal() needs to be implemented. However, all the versions of ompl::base::Goal::isSatisfied() are implemented in terms of this new function and a specified threshold (which is equal to the machine epsilon, by default).
{
public:
MyGoalRegion(const SpaceInformationPtr &si) : ompl::base::GoalRegion(si)
{
}
{
}
};
- For bi-directional planners to work, they need to know states in the goal region. If a means to sample the goal region is available, ompl::base::GoalSampleableRegion should be used. This class, which inherits from ompl::base::GoalRegion, defines two additional functions: ompl::base::GoalSampleableRegion::sampleGoal() and ompl::base::GoalSampleableRegion::maxSampleCount(). These functions allow the planners to sample goals and to tell how many different samples can be obtained in the goal region at most, respectively. Keep in mind that ompl::base::GoalRegion::distanceGoal() still needs to be implemented.
At this point, it is interesting to remark that bi-directional planners do not need to check whether a state is in the goal region, since they start with such states, so calls to ompl::base::Goal::isSatisfied() are not made. Instead, when the trees are connected, a call is made to ompl::base::Goal::isStartGoalPairValid(). This function, which by default always returns true, tells the planner whether a particular start state should be considered as forming a valid path if connected to a particular goal state. Typically, changing this function is not needed. There are a few implementations of ompl::base::GoalSampleableRegion available, for convenience:
Using the goal region
Planners cast the specified goal representation into the minimal representation they can use. For uni-directional planners, ompl::base::Goal is fine. For bi-directional planners however, this needs to be ompl::base::GoalSampleableRegion.
If the planner can use the goal specification, it will compute a motion plan. If successful, the found path is stored in the goal region. Flags indicating whether the solution was approximate are also set. The user can query all the information the planner has set using accessors from ompl::base::Goal.
Planners that include the notion of path length, and attempt to find solution paths that are shorter, also make use of a third version of ompl::base::Goal::isSatisfied(), the one that takes a path length as argument as well. This version decides whether a maximum path length is met. The user can set this maximum length by calling ompl::base::Goal::setMaximumPathLength().