Plugin Interfaces

Plugin Interfaces#

The base class of every plugin role but Map and Layer (Map, Layers), and the abstract bases and capabilities a new plugin can build on. A new plugin derives from one of these, or from a built-in implementation, and registers itself; see Extending Open Apero. The hooks no shipped plugin overrides are listed in Extension Hooks.

Robot Model#

class RobotModel : public open_apero::Plugin #

#include <open_apero_core/plugin/robot_model/robot_model.h>

Abstract robot interface: kinematic parameters and traversability/planning hooks.

Stores velocity limits, time step and the sensor-to-base transform. Subclasses define what “traversable” means for their body and which map type they query (SimpleRobot: TsdfMap).

Subclassed by open_apero::robot_model::SimpleRobot

Public Functions

virtual void attachSharedGainAnchors(const Eigen::Vector3d &position, const NodeClusterPtrs &cluster) const#

Gives the nodes of one same-position cluster gain anchors they share.

RRAG calls it once per new cluster, after the cluster’s nodes exist and before they are evaluated, with one node per candidate in sampleOrientations order. An override appends the anchors to the nodes’ gain_anchors, so an evaluator can score an anchor once for every node holding it. The default attaches none.

Parameters:

position – Sensor position of the cluster in world frame.
cluster – The cluster’s nodes.

void checkOrientations(const Eigen::Vector3d &position, const std::vector<Eigen::Quaterniond> &candidates, std::vector<RobotState> *states, std::vector<bool> *is_traversable, StationingCheck stationing = StationingCheck::kEnforce) const#

Builds robot states and checks traversability for a list of orientation candidates.

The check is isStateSafe, or isStateTraversable without the stationing rule under kExemptBootstrap. At a position failing isStationingPositionAdmissible every candidate is untraversable. states is filled for every candidate whatever the verdict. A kTraversable or kUntraversable placement is the verdict without a map query; otherwise an orientation-independent body reuses the first candidate’s verdict.

Parameters:

position – Sensor position in world frame.
candidates – Candidate orientation quaternions (e.g. from sampleOrientations).
states – Output robot states, one per candidate.
is_traversable – Output per-candidate traversability (true = traversable).
stationing – Whether to enforce the stationing rule; see StationingCheck.

virtual bool checkParamsValid(std::string *error_message) override#: Validates that all required base parameters are set.

inline const Transform3d &getBaseInSensorTransform() const#: Sensor-to-base transform (base pose expressed in the sensor frame).

virtual bool getBodyBounds(const RobotState &state, Eigen::Vector3d *min, Eigen::Vector3d *max) const#

Axis-aligned bounds of the collision geometry at state, safety margins included.

Everything the traversability and clearance verdicts at state read from the map lies inside. Default: no bounds (false); a caller then treats every map update as relevant.

inline double getClearanceMarginEps() const#

Floor of the relative clearance scale (0 < eps <= 1).

getStateClearance never reports less, so 1/clearance stays finite.

inline double getGazePitchMax() const#: Highest pitch this robot’s gaze may be planned at [rad], positive down.

inline double getGazePitchMin() const#

Lowest pitch this robot’s gaze may be planned at [rad], positive down.

Every consumer that chooses a pitch stays within [getGazePitchMin, getGazePitchMax]; the default band is the full [-pi/2, pi/2].

virtual EdgeList getInplaceAdjacency(const std::vector<RobotState> &states) const#

Returns edges connecting angularly adjacent states within a same-position cluster.

Each pair (i, j) is an in-place rotation edge. The default is the yaw ring in both directions; robots sampling more DOFs (e.g. pitch) override it.

Parameters:: states – RobotModel states from a single cluster (size must equal num_orientation_candidates).
Returns:: Edge list for in-place rotation edges within the cluster.

Map *getMap() const#: Returns the planner’s map.

inline double getMaxLinearVelocity() const#: Maximum linear velocity (m/s).

inline double getMaxYawRate() const#: Maximum yaw rate (rad/s).

inline double getSafeRadius() const#: Configured safe radius around the initial sensor position [m], 0 if unused.

inline SensorMountType getSensorMountType() const#: Sensor mounting type (kFixed, kFreePitch, kUnconstrained).

virtual double getStateClearance(const RobotState &state, Map *map) const#

Returns how much of the robot’s preferred obstacle margin a state keeps.

Relative, not a distance: 1.0 means the whole body keeps its preferred margin, lower values mean the margin is partly used up. Independent of traversability. The default reports 1.0 (a robot that declares no margin).

Parameters:

state – RobotModel state to measure.
map – Map used for environment queries.

Returns:

Relative clearance in [clearance_margin_eps, 1.0].

inline double getTimeStep() const#: Time step between consecutive trajectory states (s).

virtual EdgeList getTranslationAdjacency(const std::vector<RobotState> &from_states, const std::vector<RobotState> &to_states, const Eigen::Vector3d &departure_direction, const Eigen::Vector3d &arrival_direction, bool connect_enclosing_cell) const#

Returns edges between two clusters, scored against a planned path’s own end directions.

Without a planned path, pass the straight-line chord as both directions. The default connects the single best-aligned pair and ignores connect_enclosing_cell.

Parameters:

from_states – RobotModel states of the source cluster.
to_states – RobotModel states of the target cluster.
departure_direction – World-frame direction the path leaves from_states along.
arrival_direction – World-frame direction the path arrives at to_states along.
connect_enclosing_cell – Whether to connect every candidate bracketing the direction.

Returns:

Edge list of (from_index, to_index) pairs to connect; empty if either cluster is empty.

inline void getVisualization(const RobotTrajectory &trajectory, VisualizationMarkers *markers) const#

Generates visualization markers for the robot’s collision model along a trajectory.

Checks p_visualize_ and dispatches to getVisualizationImpl. Subclasses override getVisualizationImpl to render their specific collision geometry.

Parameters:

trajectory – The trajectory whose robot poses to visualize.
markers – Output marker container.

virtual bool isInsideHitRegions(const RobotState &robot_state, const GeometryPrimitiveCollection &hit_regions) const#

Tests whether this robot state intersects registered hit regions.

The default reports no intersection; robot models override it with their body geometry.

Parameters:

robot_state – The robot state to test.
hit_regions – Registered blocked regions.

Returns:

true if the state should be considered blocked by hit regions.

bool isStateSafe(const RobotState &state, Map *map) const#

Returns whether a state is traversable and keeps the robot’s full preferred margin.

Applied to stationing positions (nodes); trajectories are held to isStateTraversable only.

Parameters:

state – RobotModel state to check.
map – Map used for environment queries.

Returns:

true if the state is traversable and at full clearance.

bool isStateTraversable(const RobotState &state, Map *map) const#

Returns whether a robot state is traversable in the given map.

Wraps getStateTraversabilityRaw. kPotentiallyTraversable counts as traversable only within safe_radius of the initial sensor position.

Parameters:

state – RobotModel state to check.
map – Map used for environment queries.

Returns:

true if the state is considered traversable.

virtual bool isStationingPositionAdmissible(const Eigen::Vector3d &position, Map *map) const#

Returns whether the robot may be stationed (stop, rotate, observe) at a position.

Asked of a bare position, independently of traversability; trajectories are never held to it. checkOrientations enforces it at node creation, and a generator may re-ask it when it refreshes existing clusters. The default admits everywhere.

Parameters:

position – Sensor position in world frame.
map – Map used for environment queries.

Returns:

true if a node may be placed and kept at position.

virtual bool isTrajectoryTraversable(const RobotTrajectory &trajectory, Map *map) const#

Returns whether every state in a trajectory is traversable.

Parameters:

trajectory – RobotModel trajectory to check.
map – Map used for environment queries.

Returns:

true if all states are traversable.

bool isWithinSafeRadius(const RobotState &state) const#

True iff state lies within safe_radius of the initial sensor position.

The predicate isStateTraversable uses; the reference is AgentInterface::getInitialSensorPosition, not the current pose. False when safe_radius <= 0.

virtual Traversability placeRobotAtSensorPose(RobotState *robot_state, const Transform3d &sensor_transform, Map *map = nullptr) const = 0#

Places the robot so its sensor is at the given pose; robot_state is always written.

kTraversable / kUntraversable vouch for the isStateTraversable verdict, including every planner constraint it applies (hit regions, bounding volume); kUnset makes no claim and the caller must ask isStateTraversable. Never returns kPotentiallyTraversable.

Parameters:

robot_state – Output robot state to populate (always written).
sensor_transform – Sensor transform in world frame.
map – Optional planner map for map-aware placement and vouching.

Returns:

Placement-layer traversability verdict (see above).

virtual bool planRotationTrajectory(const RobotState &start, const RobotState &goal, Map *map, RobotTrajectory *trajectory)#

Plans a rotation-only trajectory connecting two same-position states.

The default steers and, when traversability depends on orientation, validates the result.

Parameters:

start – Start robot state.
goal – Goal robot state (same position, different orientation).
map – Map used for collision and traversability queries.
trajectory – Output trajectory, valid only when the function returns true.

Returns:

true if a feasible rotation trajectory was found, false otherwise.

virtual bool planTrajectory(const RobotState &start, const RobotState &goal, Map *map, RobotTrajectory *trajectory) = 0#

Plans a collision-free translation trajectory from start to goal.

Precondition: start and goal differ in position (in-place rotations go through planRotationTrajectory); implementations may throw otherwise. The result satisfies all planner constraints (bounding volume, hit regions).

Parameters:

start – Start robot state.
goal – Goal robot state.
map – Map used for collision and traversability queries.
trajectory – Output trajectory, valid only when the function returns true.

Returns:

true if a feasible trajectory was found, false otherwise.

virtual void refineBeforeExecute(RobotTrajectory &trajectory, int num_committed_states)#

Hook called by the agent on the selected edge trajectory immediately before execution.

May rewrite intermediate states (1..N-2), never front() or back(); the agent recomputes the edge cost afterwards. Only the first num_committed_states are executed before the next call, so phase carried across calls advances by those, not by the whole trajectory.

Parameters:

trajectory – Trajectory to refine in-place.
num_committed_states – Leading states executed before refinement.

inline virtual void resetEpisode() override#: Reset per-episode robot state.

bool retargetTrajectoryEndpoints(RobotTrajectory *trajectory, const RobotState &start_state, const RobotState &goal_state)#

Re-aims both end rotations of an already validated trajectory at new endpoint states.

Precondition: trajectory is validated against the current map and start_state / goal_state share its first and last positions (throws otherwise). All or nothing; an orientation-dependent robot re-checks the result against the map.

Parameters:

trajectory – Trajectory to retarget in place. Must be non-null and non-empty.
start_state – Desired first state (its position must match the trajectory’s).
goal_state – Desired last state (its position must match the trajectory’s).

Returns:

True on success, with trajectory replaced by the retargeted result. False on refusal, with trajectory unchanged.

virtual bool retargetTrajectoryHead(RobotTrajectory *trajectory, const RobotState &goal_state)#

Reorients the trajectory’s leading rotation toward goal_state.

Implemented by reversing the trajectory, calling retargetTrajectoryTail, then reversing back.

Parameters:

trajectory – Trajectory to modify in place. Must be non-null and non-empty.
goal_state – Desired initial robot state (position is ignored; only orientation is used).

Returns:

True on success. False if the result would have fewer than 2 states; the trajectory is left unchanged (in its original order).

virtual bool retargetTrajectoryTail(RobotTrajectory *trajectory, const RobotState &goal_state)#

Replaces the trailing rotation with one toward goal_state's orientation.

Keeps the trajectory up to its first state at the final position, then steers the rotation to goal_state. Refuses a result with fewer than 2 states (a zero-motion edge).

Parameters:

trajectory – Trajectory to modify in place. Must be non-null and non-empty.
goal_state – Desired final robot state (position is ignored; only orientation is used).

Returns:

True on success. False if the result would have fewer than 2 states; the trajectory is left unchanged.

explicit RobotModel(AgentInterface &agent)#: Constructs RobotModel and binds it to the agent.

virtual void sampleOrientations(const Eigen::Vector3d &position, std::vector<Eigen::Quaterniond> *candidates) const#

Populates candidates with the orientations this robot considers at a position.

The default yields num_orientation_candidates evenly spaced yaws. Performs no map queries; pair it with checkOrientations.

Parameters:

position – Sensor position in world frame.
candidates – Output list of candidate orientation quaternions.

virtual bool samplePosition(Eigen::Vector3d *pos, double z_default) const#

Samples a random position within the robot’s operating bounds.

The default returns false: the base class has no sampling bounds. SimpleRobot samples within its configured bounds and draws z only when its sample_z is true; otherwise z is z_default.

Parameters:

pos – Output sampled position.
z_default – Z value used when the robot is configured for 2D sampling.

Returns:

true if a position was sampled; false if no bounds are configured.

virtual bool samplePositionNear(const Eigen::Vector3d &center, double max_distance_3d, double max_distance_xy, Eigen::Vector3d *pos, double z_default) const#

Samples a random position in the neighborhood of a point of interest.

Same bounds and z convention as samplePosition, further restricted to within max_distance_3d of center in 3D and max_distance_xy horizontally (both apply). The default returns false, like samplePosition().

Parameters:

center – Point to sample around, in world frame.
max_distance_3d – Maximum 3D distance from center.
max_distance_xy – Maximum horizontal distance from center.
pos – Output sampled position.
z_default – Z value used when the robot is configured for 2D sampling.

Returns:

true if a position was sampled; false if no bounds are configured or the neighborhood holds no admissible position.

inline virtual bool samplesHeight() const#: Whether sampled positions vary in height; false for a robot that plans in a plane. Default: true.

inline void setBaseInSensorTransform(const Transform3d &base_in_sensor_transform)#: Override the sensor-to-base transform (e.g. when measured from TF at runtime).

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#: Loads kinematic, sensor-mount, and traversability settings.

virtual RobotTrajectory steer(const RobotState &start, const RobotState &goal, Map *map = nullptr) const = 0#

Steers from start toward goal, returning a kinematically feasible trajectory.

Precondition: the sensor poses differ (isSameSensorPose at default kEpsilon); coincident poses throw. The result therefore has at least 2 states.

Postcondition: the last state is exactly goal's sensor pose; consecutive states differ at the default pose tolerance; no step exceeds max_linear_velocity or max_yaw_rate times time_step.

Parameters:

start – The start robot state.
goal – The goal robot state (must differ from start in sensor pose).
map – Optional planner map for map-aware trajectory generation.

Returns:

A dense trajectory of robot states advancing by dt per step.

inline virtual void update()#

Refresh per-planning-cycle robot state, the robot-side peer of Layer::update().

Called once per cycle, after the executed edge’s map integration is visible and before any layer updates, expands or plans against this robot. Map-derived caches are refreshed here.

virtual void updateHitRegions(const RobotState &target_state, const RobotState &stopped_state, GeometryPrimitiveCollection *hit_regions) const#

Updates registered hit regions after a failed motion.

The map owns the storage; the robot owns the policy (removing stale regions and adding newly blocked ones). The default does nothing.

Parameters:

target_state – The robot’s target/planned state.
stopped_state – The state where the robot actually stopped.
hit_regions – Registered hit regions to update in place.

virtual ~RobotModel() = default#

Protected Functions

virtual Traversability getStateTraversabilityRaw(const RobotState &state, Map *map) const = 0#

Raw traversability of a robot state — subclasses implement collision/footprint here.

Should NOT apply the safe_radius optimism — that is layered on by the public isStateTraversable wrapper so every call site sees consistent semantics.

inline virtual void getVisualizationImpl(const RobotTrajectory&, VisualizationMarkers*) const#

Override to render robot-specific collision geometry (spheres, footprint polygons, etc.).

Called by getVisualization when p_visualize_ is true. Default is a no-op.

virtual bool isTraversabilityOrientationRelated() const = 0#

Returns whether traversability checking depends on the robot’s yaw orientation.

Returns:: true if collision geometry is orientation-dependent.

Protected Attributes

Transform3d p_base_in_sensor_transform_ = {Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity()}#

double p_clearance_margin_eps_ = {1e-4}#: Floor of the relative clearance scale; keeps 1/clearance finite.

double p_gaze_pitch_max_ = {M_PI_2}#: rad, highest plannable gaze pitch; positive looks down.

double p_gaze_pitch_min_ = {-M_PI_2}#: rad, lowest plannable gaze pitch; negative looks up.

double p_max_linear_velocity_ = {1.0}#: m/s, maximum linear velocity.

double p_max_yaw_rate_ = {M_PI / 2.0}#: rad/s, maximum yaw rate.

int p_num_orientation_candidates_ = {12}#: Number of orientation candidates the robot enumerates per position.

double p_safe_radius_ = {0.0}#

Optimism applied on top of kPotentiallyTraversable map results.

Trust positions within this distance from the initial sensor position.

SensorMountType p_sensor_mount_type_ = {SensorMountType::kFixed}#

double p_time_step_ = {0.05}#: s, time step between consecutive trajectory states.

bool p_visualize_ = {true}#

enum class open_apero::StationingCheck#

Whether checkOrientations enforces the stationing rule at a position.

kExemptBootstrap is only for the planning start node the robot already occupies; every sampled position uses kEnforce, the default.

Values:

enumerator kEnforce#

enumerator kExemptBootstrap#

Cost Computer#

class CostComputer : public open_apero::Plugin #

#include <open_apero_core/plugin/cost_computer/cost_computer.h>

Abstract base class for computing the cost of traversing an edge.

Implementations define the cost metric used to evaluate edges in the planning graph (e.g., distance, time, energy).

Subclassed by open_apero::cost_computer::IdentityCost, open_apero::cost_computer::TrajectoryLength, open_apero::cost_computer::TrajectoryTime

Public Functions

virtual bool computeCost(Edge *edge) = 0#

Compute the cost of traversing the given edge.

Parameters:: edge – Pointer to the edge whose cost should be computed.
Returns:: True if the cost was computed successfully.

inline explicit CostComputer(AgentInterface &agent)#

Construct a CostComputer associated with the given agent.

Parameters:: agent – Reference to the agent interface.

inline virtual double getCostLowerBound(const RobotState&, const RobotState&)#

A cost no edge the robot plans from start to goal can undercut.

Lets a search skip planning an edge that cannot win. Default: 0, which skips nothing.

inline virtual void resetEpisode() override#: Reset per-episode cost computer state.

Generators#

class Generator : public open_apero::Plugin #

#include <open_apero_core/plugin/generator/generator.h>

Abstract base class for sample-based node generators.

Provides the KD-tree, the sampling interface, and collision-checking access to the robot model; subclasses implement a sampling strategy.

Subclassed by open_apero::GraphGenerator

Public Functions

virtual bool containsNode(const Node *node) const#

Whether node is still a member of this generator’s structure.

Pointer-only check: must not dereference node, which may already be freed. The default answers over the KD-tree index; a generator inheriting it must rebuild the index in the same operation that releases a node. Generators owning nodes outside the index override it.

virtual NodeClusterPtrs generateNewNode(Node *start_node) = 0#

Samples a new candidate position and adds it as a node cluster to the index.

Parameters:: start_node – Optional sampling context, such as a graph’s start node.
Returns:: Pointers to the newly created node cluster.

explicit Generator(AgentInterface &agent)#: Constructs a Generator and binds it to the agent.

inline const std::vector<NodeClusterPtrs> &getAllNodeClusters() const#

Returns all non-intermediate node clusters in the KD-tree, grouped by position.

Each entry is a cluster of nodes sharing the same 3D location. The returned reference is invalidated if the KD-tree is rebuilt (e.g. after rebuildKdTree()).

Returns:: Const reference to the vector of node clusters.

inline Layer *getLayer() const#: Owning layer, or nullptr if not yet wired. Used to reach sibling plugins.

inline Map *getMap() const#: Returns the agent map used by this generator.

virtual void getVisualization(VisualizationMarkers *markers)#

Generates gain-colored markers for all sampled (non-intermediate) nodes in the KD-tree.

Parameters:: markers – Output marker container.

virtual bool rebuildKdTree() = 0#: Rebuilds the KD-tree for the current node set.

virtual void resetEpisode() override#: Resets sampling-side state (KD-tree) for a new episode.

inline void setLayer(Layer *layer)#: Sets the owning layer; called by the owning layer after setup. getLayer() is nullptr during setupFromParamMap.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#: Binds the agent’s robot model; the base reads no keys.

virtual ~Generator() = default#

Protected Functions

void addToKdTree(const NodeClusterPtrs &cluster)#

Adds a freshly built node cluster to the KD-tree.

All nodes in cluster must share a 3D position (they map to a single KD-tree point with multiple stored Node pointers).

Parameters:: cluster – The cluster to register.

bool findNearbyNodes(const Eigen::Vector3d &point, std::vector<NodeClusterPtrs> &nearby_node_clusters, double max_length)#

Finds all nearby node clusters within max_length of a point.

Parameters:

point – Query point.
nearby_node_clusters – Output node clusters in KD-tree traversal order.
max_length – Maximum search radius.

Returns:

true if any nearby nodes were found.

bool findNearestNode(const Eigen::Vector3d &point, NodeClusterPtrs &nearest_node_cluster, double &dist_squared)#

Finds the nearest node cluster to a point in the KD-tree.

Parameters:

point – Query point.
nearest_node_cluster – Output nearest node cluster.
dist_squared – Output squared distance to the nearest cluster.

Returns:

true if a nearest node was found.

bool isAffectedByMapUpdate(const NodeClusterPtrs &cluster, const Eigen::Vector3d &reach = Eigen::Vector3d::Zero()) const#: isAffectedByMapUpdate() over the nodes’ robot states.

bool isAffectedByMapUpdate(const RobotTrajectory &states, const Eigen::Vector3d &reach = Eigen::Vector3d::Zero()) const#

Whether the map region updated since the last cycle can change the robot’s verdict at any of states.

True when the updated region meets the body bounds (RobotModel::getBodyBounds) of a state, grown by reach per axis, or when the robot reports no bounds. False without a map update.

Parameters:

states – The states to check, e.g. an edge’s trajectory.
reach – How far beyond the bodies a change still matters [m], e.g. an edge length for a new connection.

void refreshClusterTraversability(const NodeClusterPtrs &cluster, Map *map) const#

Recomputes is_traversable for every node of one co-located cluster.

Goes through RobotModel::checkOrientations, so refresh applies exactly the creation rules (stationing included); the nodes’ states are unchanged.

Parameters:

cluster – Co-located nodes sharing one sensor position. Must be non-empty.
map – Unused by the in-tree generators: checkOrientations queries the robot’s own map.

Protected Attributes

KdTreeData kd_tree_data_#

std::unique_ptr<KdTree> kdtree_#: Spatial index for sampled node clusters.

Layer *layer_ = nullptr#

RobotModel *robot_ = nullptr#: Robot model used for sampling-time collision checks.

Protected Static Functions

static void addClusterToIndex(KdTree *kdtree, KdTreeData *data, const NodeClusterPtrs &cluster)#

Adds a cluster to an explicitly supplied spatial index.

The tree must be initialized over data. The cluster must be non-empty, non-intermediate, co-located, and not already present in the index.

static bool findNearbyNodesInIndex(const KdTree *kdtree, const KdTreeData &data, const Eigen::Vector3d &point, std::vector<NodeClusterPtrs> *nearby_node_clusters, double max_length)#

Finds nearby clusters in an explicitly supplied spatial index.

kdtree must be null or built over data. The output is always cleared first and is in KD-tree traversal order (unsorted). A radius <= 1e-2 is unbounded, as in findNearbyNodes().

static bool findNearestNodeInIndex(const KdTree *kdtree, const KdTreeData &data, const Eigen::Vector3d &point, NodeClusterPtrs *nearest_node_cluster, double *dist_squared)#

Finds the nearest cluster in an explicitly supplied spatial index.

kdtree must be null or have been constructed over data. The cluster output is cleared before every query; both outputs are populated on success.

class GraphGenerator : public open_apero::Generator #

#include <open_apero_core/plugin/generator/graph_generator.h>

Abstract base class for generating planning nodes and connecting edges.

Manages collision-free edge construction on top of the sampling/KD-tree machinery inherited from Generator. Subclasses implement the specific graph or tree expansion strategy.

The start node is the vertex the robot’s state stands on: the owning PlanningLayer’s current node, and the source of every search. A tree also calls it its root.

Subclassed by open_apero::TreeGenerator, open_apero::generator::RRAG

Public Functions

virtual bool checkParamsValid(std::string *error_message) override#

Validates the edge lengths: min_edge_length > 0, max_edge_length 0 (no limit) or >= min_edge_length.

Parameters:: error_message – Populated with a description of the first validation failure.
Returns:: true if all parameters are valid.

virtual bool computeCost(Edge *edge)#

Computes and sets the traversal cost on an edge.

Forwards to Layer::computeCost, i.e. the agent’s cost computer.

Parameters:: edge – The edge to cost.
Returns:: true on success.

inline virtual void connectStartNode(Node*)#

Gives an intermediate start node its outgoing edges to clusters within reach.

Called by the layer after expansion when the start node has no out-edges, so clusters born this cycle connect in the same cycle. No-op for a tree, whose start node is connected by construction.

virtual bool containsNode(const Node *node) const = 0#

Whether node is still a member of this generator’s structure, intermediate nodes included.

Pure virtual again: Generator’s KD-tree default excludes intermediate nodes. Pointer-only check; must not dereference node.

virtual std::pair<GraphType*, Node*> generateInitNode(const RobotState &init_state) = 0#

Creates the initial planning node from the robot’s starting state.

Parameters:: init_state – The initial robot state.
Returns:: A pair of the graph and the initial node pointer. The graph is nullptr for a tree generator (TreeGenerator), whose nodes are linked through TreeNode::parent and TreeNode::children instead.

virtual bool generateNewEdges(NodeClusterPtrs &node_cluster, std::vector<Edge*> &new_edges) = 0#

Connects a newly sampled node cluster to the existing graph with traversable edges.

Parameters:

node_cluster – The node cluster to connect.
new_edges – Output vector of newly created edges.

Returns:

true if at least one edge was successfully generated.

virtual std::vector<Edge*> getAllEdges(Node *start_node) const = 0#

Returns every edge of the structure.

Parameters:: start_node – Unused by the built-in generators.
Returns:: Vector of pointers to all edges.

virtual std::vector<Node*> getAllNodes(Node *start_node) const = 0#

Returns every node of the structure.

Parameters:: start_node – Unused by the built-in generators.
Returns:: Vector of pointers to all nodes.

virtual void getVisualization(Node *start_node, VisualizationMarkers *markers)#

Generates visualization markers for the planning graph (nodes + optional edges).

Parameters:

start_node – The current start node.
markers – Output marker container.

virtual void getVisualization(VisualizationMarkers *markers)#: Keeps Generator’s KD-tree overload visible next to the graph overload below.

explicit GraphGenerator(AgentInterface &agent)#: Constructs a GraphGenerator and binds it to the agent.

virtual std::pair<Node*, Edge*> insertIntermediateNode(Node *start_node, Node *goal_node, Edge *edge, int node_index) = 0#

Inserts an intermediate node at a specified point along an existing edge.

Parameters:

start_node – The source node of the edge.
goal_node – The target node of the edge.
edge – The edge to split.
node_index – Index of the intermediate point along the trajectory.

Returns:

A pair of the new intermediate node and the edge from start_node to it.

virtual Node *insertIntermediateNodeAtCurrentState(Node *previous, const Edge &executed_edge) = 0#

Makes the robot’s current state, reached off this structure, the new start node (an existing node at exactly that pose is adopted).

Only executed_edge.trajectory.back() is used; the edge is not imported. Never returns nullptr: without a plannable out-edge a graph keeps a detached intermediate start node and a tree starts a fresh single-node root. A graph removes the previous intermediate start node, a tree reparents it under the new one; do not re-seed.

Parameters:

previous – The generator’s current start node (already part of this structure).
executed_edge – Edge whose endpoint gives the new node’s robot state.

Returns:

Raw pointer to the new start node.

virtual bool isEdgeBidirectional() const = 0#

Returns whether the generator creates bidirectional edges.

Returns:: true for graph-based generators (edges traverse in both directions); false for tree-based generators (edges are directed from parent to child).

virtual void setNewStartNode(Node *previous_start, Node *new_start) = 0#

Updates generator state after the robot moves to a new start node.

Parameters:

previous_start – The old start node.
new_start – The new start node.

virtual bool setNodeOrientation(Node *node, const Eigen::Quaterniond &orientation) = 0#

Changes the orientation of a node and updates all connected edges.

Parameters:

node – The node to reorient.
orientation – The new orientation quaternion.

Returns:

true on success.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#: Loads edge-side parameters (min/max edge length) from the config map.

virtual void updateNodeEdge(Node *start_node) = 0#

Re-checks the nodes and edges this cycle’s map update can affect, and prunes what became untraversable.

Affected: the map region updated since the last cycle meets the robot’s body there (isAffectedByMapUpdate()).

Parameters:: start_node – The current start node.

virtual ~GraphGenerator() = default#

Protected Functions

bool buildRetargetedEdge(const Edge &seed_edge, Node *start_node, Node *goal_node, Edge *edge)#

Builds a translation edge by re-aiming a validated seed edge at another node pair.

Only the seed’s two end rotations are rebuilt. Precondition: the seed edge’s endpoints share their positions with start_node and goal_node.

Parameters:

seed_edge – Validated edge whose trajectory is reused.
start_node – Source node, supplying the new departure orientation.
goal_node – Target node, supplying the new arrival orientation.
edge – Output edge to populate.

Returns:

true if the retargeted trajectory was accepted; false otherwise.

bool generateEdgeBetweenNodes(Node *start_node, Node *goal_node, Edge *edge)#

Generates a collision-free translation edge between distinct positions.

Start and goal must have distinct base positions. Use generateRotationEdgeBetweenNodes() for coincident nodes.

Parameters:

start_node – Source node.
goal_node – Target node (distinct position from start_node).
edge – Output edge to populate.

Returns:

true if a feasible trajectory was found; false otherwise.

bool generateRotationEdgeBetweenNodes(Node *start_node, Node *goal_node, Edge *edge)#

Generates a rotation-only edge between coincident nodes (see RobotModel::planRotationTrajectory).

Parameters:

start_node – Source node.
goal_node – Target node (same position, different orientation).
edge – Output edge to populate.

Returns:

true if a feasible rotation trajectory was found; false otherwise.

void revalidateEdge(Edge *edge)#

Re-validate an existing edge against the current map (in place).

Only is_traversable is updated; the trajectory is never changed, since the executor follows it by pointer. Backtrack edges are left untouched (see Edge::is_backtrack).

Parameters:: edge – The edge to refresh (only its is_traversable flag is updated).

Protected Attributes

double p_max_edge_length_ = 100.0#

double p_min_edge_length_ = 0.8#

bool p_visualize_edges_ = false#

class TreeGenerator : public open_apero::GraphGenerator #

#include <open_apero_core/plugin/generator/tree_generator.h>

Base tree-structured node/edge generator managing a rooted exploration tree.

Subclassed by open_apero::generator::RRT

Public Functions

virtual bool containsNode(const Node *node) const override#

Whether node is still a member of this generator’s structure, intermediate nodes included.

Pure virtual again: Generator’s KD-tree default excludes intermediate nodes. Pointer-only check; must not dereference node.

virtual std::pair<GraphType*, Node*> generateInitNode(const RobotState &init_state) override#

Creates and initializes the root node of the tree at the robot’s initial state.

Parameters:: init_state – The robot’s starting pose.
Returns:: Pair of (graph pointer, root node pointer).

virtual bool generateNewEdges(NodeClusterPtrs &node_cluster, std::vector<Edge*> &new_edges) override#

Connects a newly generated node cluster into the tree by creating edges.

Parameters:

node_cluster – Cluster of candidate nodes to attach.
new_edges – Output list populated with the newly created edges.

Returns:

true on success.

virtual std::vector<Edge*> getAllEdges(Node *start_node) const override#

Returns every edge of the tree, from its root.

Parameters:: start_node – Unused.
Returns:: Flat list of all edge pointers in the tree.

virtual std::vector<Node*> getAllNodes(Node *start_node) const override#

Returns every node of the tree, from its root.

Parameters:: start_node – Unused.
Returns:: Flat list of all node pointers in the tree.

virtual std::pair<Node*, Edge*> insertIntermediateNode(Node *start_node, Node *goal_node, Edge *edge, int node_index) override#

Inserts an intermediate waypoint node along an existing edge.

Parameters:

start_node – The edge’s parent node.
goal_node – The edge’s child node.
edge – The edge to split.
node_index – Index of the trajectory waypoint at which to insert.

Returns:

Pair of (new intermediate node, the edge from start_node to it).

virtual Node *insertIntermediateNodeAtCurrentState(Node *previous, const Edge &executed_edge) override#

Insert a new intermediate node at the endpoint of executed_edge as the new tree root.

Never returns nullptr: when the edge back to previous cannot be planned, the tree is dropped for a fresh single-node root at the reached state.

Parameters:

previous – Existing tree root node; becomes a child of the new root after rerooting.
executed_edge – Edge whose endpoint gives the new root’s robot state.

inline virtual bool isEdgeBidirectional() const override#: Tree edges are unidirectional (directed from parent to child).

inline virtual void resetEpisode() override#: Resets tree state for a new episode.

virtual bool setNodeOrientation(Node *node, const Eigen::Quaterniond &orientation) override#

Changes the sensor/heading orientation of a node and updates connected edges.

Parameters:

node – The node whose orientation is being changed.
orientation – The new sensor orientation as a quaternion.

Returns:

true if the orientation change was accepted (collision-free).

explicit TreeGenerator(AgentInterface &agent)#: Constructs the TreeGenerator and binds it to the agent.

virtual void updateNodeEdge(Node *start_node) override#

Revalidates the edges a map update can affect and drops the subtrees behind untraversable ones.

Parameters:: start_node – Root of the tree to update.

virtual ~TreeGenerator() = default#

Protected Functions

virtual bool rebuildKdTree() override#

Rebuilds the kd-tree index from the current set of tree nodes.

Returns:: true on success.

void updateNodeEdgeRecursive(Node *node)#: Recursive helper for updateNodeEdge (prunes without rebuilding KD-tree).

Protected Attributes

std::unique_ptr<TreeNode> root_node_#

Evaluators#

class Evaluator : public open_apero::Plugin #

#include <open_apero_core/plugin/evaluator/evaluator.h>

Abstract base class for evaluating information gain at planning nodes.

Implementations must define evaluateUnobserved() and getRealAccumulatedGain(). Callers use evaluate() and updateEvaluation(), which pass only the unobserved nodes (Node::is_observed) on to evaluateUnobserved() and reevaluateUnobserved(), so an observed node’s gain stays spent under every evaluator.

Subclassed by open_apero::evaluator::SimulatedSensorEvaluator, open_apero::evaluator::YawPlanningEvaluator

Public Functions

bool evaluate(const NodeClusterPtrs &cluster)#

Evaluates the information gain for a cluster of candidate viewpoint nodes.

All nodes in the cluster share the same spatial position but may differ in orientation. Observed nodes are skipped; the rest go to evaluateUnobserved().

Parameters:: cluster – The candidate viewpoint nodes to evaluate.
Returns:: true on success, and when no unobserved node is left.

explicit Evaluator(AgentInterface &agent)#: Constructs an Evaluator and binds it to the agent.

inline Layer *getLayer() const#: Owning layer, or nullptr if not yet wired. Used to reach sibling plugins.

virtual void getPathVisualization(VisualizationMarkers *markers, const std::vector<Node*> &path)#

Visualizes what the selected path as a whole is being credited with.

Differs from the per-node view where gain is shared between nodes. The default draws nothing.

Parameters:

markers – Output visualization markers.
path – The selected path, in the order it is traversed.

virtual double getRealAccumulatedGain() const = 0#

Returns the total accumulated information gain across all reached nodes.

Returns:: Accumulated gain value.

virtual void getVisualization(VisualizationMarkers *markers, const Node *node)#

Visualizes the information gain at this node for simulation display.

Parameters:

markers – Output visualization markers.
node – The node to visualize.

virtual void onNodeReached(Node *reached_node, const Edge *traversed_edge)#

Called once per planning cycle after a node has been reached.

The default spends the node (Node::markObserved()). Subclasses that track visits more finely (e.g. per orientation) override it without calling the base.

Parameters:

reached_node – The node the robot just navigated to.
traversed_edge – Edge used to reach the node, or nullptr.

inline virtual void resetEpisode() override#

Resets evaluator state for a new episode. Default: no-op.

Per-node state is cleared with the graph; subclasses clear accumulated gain tracking.

inline void setLayer(Layer *layer)#: Sets the owning layer; called by the owning layer after setup. getLayer() is nullptr during setupFromParamMap.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#: Loads base Evaluator parameters (bounding volume) from the config map.

bool updateEvaluation(const NodeClusterPtrs &cluster, const Node *current_node = nullptr)#

Re-evaluates a cluster’s gain after the map has been updated.

Called during each planning cycle for every cluster in the graph. Observed nodes are skipped; the rest go to reevaluateUnobserved().

Parameters:

cluster – The nodes to update.
current_node – If non-null and present in the cluster, gain is re-evaluated but the node’s orientation is not modified.

Returns:

Whether re-evaluation succeeded, and true when no unobserved node is left. Never used to prune nodes; the generator’s traversability pruning does that.

virtual ~Evaluator() = default#

Protected Functions

virtual bool evaluateUnobserved(const NodeClusterPtrs &cluster) = 0#

Evaluates a cluster for evaluate(); fills each node’s gain anchors.

Implementations are encouraged to share expensive computation (e.g., raycasting) across cluster members.

Parameters:: cluster – Nodes of one cluster, none of them observed; evaluate() never passes an empty one.
Returns:: true on success.

virtual bool reevaluateUnobserved(const NodeClusterPtrs &cluster, const Node *current_node)#

Re-evaluates a cluster for updateEvaluation(). Default: never re-evaluates.

Parameters:

cluster – Nodes of one cluster, none of them observed; updateEvaluation() never passes an empty one.
current_node – As for updateEvaluation().

Returns:

Whether re-evaluation succeeded.

Protected Attributes

std::unique_ptr<BoundingVolume> bounding_volume_#

Layer *layer_ = nullptr#

class SimulatedSensorEvaluator : public open_apero::Evaluator #

#include <open_apero_core/plugin/evaluator/simulated_sensor_evaluator.h>

Base evaluator for simulated sensor observations.

evaluate() runs, for every unobserved node of the cluster: the sensor model’s getVisibleVoxelsFromNode(); a filter keeping the voxels inside the bounding volume (when one is configured); with clear_from_parents, removal of voxels a tree ancestor already sees; storeVisibleVoxels(), which replaces node->info with a SimulatedSensorInfo holding them; evaluateFromVisibleVoxels(), which a subclass implements to set the node’s gain from that info; and computeFrontierStatus(). updateEvaluation() re-runs evaluate() on the unobserved nodes that are traversable, worth updating (gain above update_gain or at a frontier) and pass nodeSensorViewOverlapsUpdatedMap().

Subclassed by open_apero::evaluator::NaiveEvaluator, open_apero::evaluator::PointsEvaluator, open_apero::evaluator::SurfaceFrontierEvaluator

Public Functions

virtual void getVisualization(VisualizationMarkers *markers, const Node *node) override#: Visualizes the sensor view frustum and per-voxel gain at the node.

inline virtual void resetEpisode() override#: Resets evaluator state for a new episode.

bool sensorViewOverlapsUpdatedMap(const RobotState &state)#

Tests whether the state’s sensor view overlaps the map update.

Parameters:: state – The robot state to query.
Returns:: true if there is overlap with the updated map region.

bool sensorViewOverlapsUpdatedMap(const Transform3d &sensor_transform)#

Tests whether a sensor pose overlaps the map update.

Parameters:: sensor_transform – Sensor transform in world frame.
Returns:: true if there is overlap with the updated map region.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#: Loads the sensor model and frontier thresholds.

explicit SimulatedSensorEvaluator(AgentInterface &agent)#: Constructs a SimulatedSensorEvaluator and binds it to the agent.

Protected Functions

void computeFrontierStatus(Node *node)#

Computes the frontier status of a node from its visible voxel occupancy.

Parameters:: node – The node whose frontier status will be updated.

virtual bool evaluateFromVisibleVoxels(Node *node) = 0#

Computes the scalar gain from the node’s stored visible voxel data.

Parameters:: node – The node to evaluate.
Returns:: true on success.

virtual bool evaluateUnobserved(const NodeClusterPtrs &cluster) override#: Evaluates gain from simulated visible voxels.

virtual bool nodeSensorViewOverlapsUpdatedMap(const Node *node)#

Tests whether the map update can affect what a node observes.

The default tests the node’s own sensor pose; evaluators that raycast other poses (e.g. gain anchors with their own orientations) must override it.

Parameters:: node – The node about to be re-evaluated.
Returns:: true if the node still has to be re-evaluated.

virtual bool reevaluateUnobserved(const NodeClusterPtrs &cluster, const Node *current_node) override#: Re-evaluates eligible nodes after map changes.

virtual bool storeVisibleVoxels(Node *node, const VoxelOccupancyMap &new_voxels)#

Stores the newly visible voxels into the node’s data structure.

Parameters:

node – The node to update.
new_voxels – The voxel occupancy map from ray casting.

Returns:

true on success.

Protected Attributes

map::OccupancyMap *map_ = nullptr#: The agent’s map as an OccupancyMap; nullptr when the map is of another type.

int max_num_visible_voxels_ = 0#

bool p_clear_from_parents_ = false#

double p_frontier_min_unknown_ratio_ = 0.0#

double p_frontier_min_visible_ratio_ = 0.0#

bool p_only_update_for_updated_map_ = true#: If false, updates every node.

double p_update_gain_ = 0.0#: Updates only nodes above this gain.

bool p_visualize_sensor_view_ = false#

std::unique_ptr<SensorModel> sensor_model_#

class YawPlanningEvaluator : public open_apero::Evaluator #

#include <open_apero_core/plugin/evaluator/yaw_planning_evaluator.h>

Base evaluator for testing and selecting yaw orientations.

Subclassed by open_apero::evaluator::SimpleYawPlanningEvaluator

Public Functions

virtual void getVisualization(VisualizationMarkers *markers, const Node *node) override#: Visualizes the gain at the best yaw orientation found for this node.

virtual void onNodeReached(Node *reached_node, const Edge *traversed_edge) override#

Marks the active orientation as visited and forwards to the following evaluator.

Parameters:

reached_node – The node the robot just navigated to.
traversed_edge – Edge used to reach the node, or nullptr.

inline virtual void resetEpisode() override#

Resets evaluator state for a new episode. Default: no-op.

Per-node state is cleared with the graph; subclasses clear accumulated gain tracking.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#

Loads yaw settings and creates the delegated evaluator.

Throws:: std::runtime_error – if the following evaluator is not a SimulatedSensorEvaluator.

explicit YawPlanningEvaluator(AgentInterface &agent)#: Constructs a YawPlanningEvaluator and binds it to the agent.

Protected Functions

virtual bool evaluateUnobserved(const NodeClusterPtrs &cluster) override#

Tests multiple yaw orientations and delegates gain computation to the following evaluator.

Parameters:: cluster – The candidate viewpoint nodes.
Returns:: true on success.

virtual bool reevaluateUnobserved(const NodeClusterPtrs &cluster, const Node *current_node) override#: Re-evaluates gains across all sampled yaw orientations when the map changes. If current_node matches the cluster, gains are refreshed without changing yaw.

virtual bool setBestYaw(Node *node)#

Samples yaw orientations and sets the one with highest gain on the node.

Parameters:: node – The node to optimize.
Returns:: true on success.

virtual bool setNodeOrientation(Node *node, const Eigen::Quaterniond &orientation)#

Sets the sensor orientation of a node and updates its trajectory.

Parameters:

node – The node to reorient.
orientation – The new sensor orientation quaternion.

Returns:

true on success.

Protected Attributes

std::unique_ptr<Evaluator> following_evaluator_#

double p_update_gain_ = 0.0#

SeededRng rng_#

SimulatedSensorEvaluator *sensor_evaluator_ = nullptr#: following_evaluator_ as the SimulatedSensorEvaluator setup requires it to be.

Sensor Model#

class SensorModel : public open_apero::Plugin #

#include <open_apero_core/plugin/sensor_model/sensor_model.h>

Abstract base class for sensor simulation via ray casting.

Provides the interface for computing visible voxels from a given sensor pose. Concrete implementations (SimpleRayCaster, IterativeRayCaster, etc.) define the ray casting strategy and sensor geometry.

Subclassed by open_apero::sensor_model::CameraModel, open_apero::sensor_model::LidarModel

Public Functions

virtual void getSensorViewBoundary(Eigen::Vector3d &min, Eigen::Vector3d &max, const Transform3d &sensor_transform) = 0#

Returns the axis-aligned bounding box of the sensor’s visible volume at a given pose.

Parameters:

min – Output minimum corner of the bounding box.
max – Output maximum corner of the bounding box.
sensor_transform – Sensor transform in world frame.

virtual bool getVisibleVoxels(VoxelOccupancyMap *result, const Transform3d &sensor_transform) = 0#

Returns visible voxel centers and their occupancy states for an explicit sensor pose.

Parameters:

result – Output map of voxel positions to occupancy states.
sensor_transform – Sensor transform in world frame.

Returns:

true on success.

virtual bool getVisibleVoxelsFromNode(VoxelOccupancyMap *result, const Node *node_in)#

Returns voxels visible from a planning node’s sensor pose.

Parameters:

result – Output map of voxel positions to occupancy states.
node_in – The planning node whose sensor pose is used.

Returns:

true on success.

virtual bool pointInSensorView(const Eigen::Vector3d &point, const Transform3d &sensor_transform, double margin = 0.0) const#

Whether a world point lies inside the sensor’s visible volume at a pose.

Range and field of view only, no occlusion. margin grows the volume (half a voxel diagonal makes the test conservative for voxels). The default reports every point visible.

Parameters:

point – Query point in world frame.
sensor_transform – Sensor transform in world frame.
margin – Distance by which to expand the view volume.

Returns:

true if point lies in the view volume.

inline virtual void resetEpisode() override#: Reset per-episode sensor model state.

explicit SensorModel(AgentInterface &agent)#: Constructs a SensorModel and binds it to the agent.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#

Loads base sensor model parameters (ray length, log_cast_timing) from the config map.

Throws:: std::runtime_error – if the agent’s map is not an OccupancyMap.

virtual void visualizeSensorView(VisualizationMarkers *markers, const Node *node) = 0#

Generates visualization markers showing the sensor’s visible volume at a node.

Parameters:

markers – Output visualization markers.
node – The node whose sensor view to visualize.

virtual ~SensorModel() = default#

Protected Attributes

map::OccupancyMap *map_ = nullptr#: Occupancy map used for ray queries: the agent’s map, which setup requires to be an OccupancyMap.

bool p_log_cast_timing_ = false#: Time every getVisibleVoxelsFromNode() cast and log the mean and spread every 20 casts.

double p_ray_length_ = 5.0#

std::vector<double> time_count_#: Cast durations [ms] this episode, guarded by time_count_mutex_.

std::mutex time_count_mutex_#

Protected Static Functions

static void recordVoxel(VoxelOccupancyMap *result, const Eigen::Vector3d &center, Occupancy occupancy)#

Records a voxel a ray reached; kOccupied overrides an earlier reading of the voxel.

Parameters:

result – Visible voxels of the cast.
center – Voxel center in world frame.
occupancy – Occupancy the ray read.

class CameraModel : public open_apero::SensorModel #

#include <open_apero_core/plugin/sensor_model/camera_model.h>

Camera model for visibility and view-boundary queries.

Subclassed by open_apero::sensor_model::IterativeRayCaster, open_apero::sensor_model::SimpleRayCaster

Public Functions

explicit CameraModel(AgentInterface &agent)#: Constructs a CameraModel and binds it to the agent.

virtual bool checkParamsValid(std::string *error_message) override#

Validates that focal length and resolution parameters are positive.

Parameters:: error_message – Populated with a description of the first validation failure.
Returns:: true if all parameters are valid.

virtual void getSensorViewBoundary(Eigen::Vector3d &min, Eigen::Vector3d &max, const Transform3d &sensor_transform) override#

Returns the axis-aligned bounding box of the camera’s view, every ray out to the observation boundary; exact for any orientation.

Parameters:

min – Output minimum corner.
max – Output maximum corner.
sensor_transform – Sensor transform in world frame.

virtual bool pointInSensorView(const Eigen::Vector3d &point, const Transform3d &sensor_transform, double margin = 0.0) const override#

Whether a world point falls within the observation boundary and inside the frustum.

Pinhole test against the same range and field of view the ray caster uses, grown by margin in every direction, so it never rejects a point the ray caster would have reported.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#

Loads camera parameters (focal length, resolution, observation boundary) from the config map.

Throws:: std::runtime_error – if observation_boundary is neither “spherical” nor “planar”.

virtual void visualizeSensorView(VisualizationMarkers *markers, const Node *node) override#: Visualizes the camera frustum boundaries at the node’s sensor pose.

virtual ~CameraModel() = default#

Protected Types

enum class ObservationBoundary#

Surface that ends the view; the names and spelling are Voxblox’s observation_boundary.

Values:

enumerator kSpherical#: The sphere of radius ray_length around the sensor (default).

enumerator kPlanar#: The plane at camera depth ray_length, as a depth camera’s max range.

Protected Functions

void getDirectionVector(Eigen::Vector3d *result, double relative_x, double relative_y)#

Computes the ray direction for a pixel at relative image coordinates.

Parameters:

result – Output direction vector (normalized).
relative_x – Relative x position in [0, 1].
relative_y – Relative y position in [0, 1].

double getObservationBoundaryWidth(double field_of_view, int resolution) const#

Extent of the observation boundary along one image axis: the arc ray_length * field_of_view on the sphere, or the far face’s ray_length * resolution / focal_length on the plane.

Parameters:

field_of_view – The axis’s field of view [rad].
resolution – The axis’s resolution [px].

double getRayLength(const Eigen::Vector3d &camera_direction) const#

Distance along a ray at which the view ends.

Parameters:: camera_direction – Unit ray direction in the camera frame, as getDirectionVector returns it.
Returns:: ray_length, or for a planar boundary the distance at which the ray reaches camera depth ray_length.

Protected Attributes

double c_field_of_view_x_ = 0.0#

double c_field_of_view_y_ = 0.0#

double c_sec_half_field_of_view_x_ = 1.0#: Converts a distance measured perpendicular to a frustum side plane into the lateral offset it allows at a given depth. See pointInSensorView.

double c_sec_half_field_of_view_y_ = 1.0#

double c_tan_half_field_of_view_x_ = 0.0#

double c_tan_half_field_of_view_y_ = 0.0#

double p_focal_length_ = 320.0#

ObservationBoundary p_observation_boundary_ = ObservationBoundary::kSpherical #

int p_resolution_x_ = 640#

int p_resolution_y_ = 480#

class LidarModel : public open_apero::SensorModel #

#include <open_apero_core/plugin/sensor_model/lidar_model.h>

Lidar model for visibility and view-boundary queries.

Subclassed by open_apero::sensor_model::IterativeRayCasterLidar

Public Functions

virtual bool checkParamsValid(std::string *error_message) override#

Validates that FOV and resolution parameters are positive.

Parameters:: error_message – Populated with a description of the first validation failure.
Returns:: true if all parameters are valid.

virtual void getSensorViewBoundary(Eigen::Vector3d &min, Eigen::Vector3d &max, const Transform3d &sensor_transform) override#

Returns the axis-aligned bounding box of the lidar scan volume; a scan of at least half a turn is bounded as from a level sensor (ray_length horizontally around it).

Parameters:

min – Output minimum corner.
max – Output maximum corner.
sensor_transform – Sensor transform in world frame.

explicit LidarModel(AgentInterface &agent)#: Constructs a LidarModel and binds it to the agent.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#: Loads lidar parameters (FOV, resolution) from the config map.

virtual void visualizeSensorView(VisualizationMarkers *markers, const Node *node) override#: Visualizes the lidar scan volume boundaries at the node’s sensor pose.

virtual ~LidarModel() = default#

Protected Functions

void getDirectionVector(Eigen::Vector3d *result, double relative_x, double relative_y)#

Computes the ray direction for a scan point at relative angular coordinates.

Parameters:

result – Output direction vector (normalized).
relative_x – Relative horizontal position in [0, 1].
relative_y – Relative vertical position in [0, 1].

Protected Attributes

double p_field_of_view_x_ = 2.0 * M_PI#: Total field of view [rad], symmetric about the facing direction; configured in degrees.

double p_field_of_view_y_ = 30.0 * M_PI / 180.0#: Vertical field of view [rad]; configured in degrees.

int p_resolution_x_ = 1024#

int p_resolution_y_ = 128#

Path Planner#

class PathPlanner : public open_apero::Plugin #

#include <open_apero_core/plugin/path_planner/path_planner.h>

Abstract base class that plans the next best path over the planning graph.

Selects among candidate paths from the start node by the configured Objective. The cost budget is the agent’s (AgentInterface::getCurrentCostBudget).

Subclassed by open_apero::path_planner::GraphBeamSearch, open_apero::path_planner::GraphShortestPathTreeSearch, open_apero::path_planner::GraphTspSearch, open_apero::path_planner::NoOpPathPlanner, open_apero::path_planner::TreeBestValue

Public Functions

virtual bool checkParamsValid(std::string *error_message) override#: Validates gain_suppression_radius: finite and >= 0.

inline Layer *getLayer() const#: Owning layer, or nullptr if not yet wired. Used to reach sibling plugins.

inline explicit PathPlanner(AgentInterface &agent)#

Construct a PathPlanner associated with the given agent.

Parameters:: agent – Reference to the agent interface.

inline std::pair<std::vector<Node*>, double> planPath(Node *start_node, GraphType *graph)#

Select the next best path from the given start node.

Answers “where should I go next”: any prefix the objective likes is a legal answer.

Parameters:

start_node – The node to start path selection from.
graph – Pointer to the planning graph; nullptr when the layer’s generator is a tree (TreeGenerator).

Returns:

A pair containing the ordered sequence of nodes forming the best path and its value.

virtual std::pair<std::vector<Node*>, double> planPath(Node *start_node, GraphType *graph, Node *required_terminal_node) = 0#

Select the best path from start_node that ends at required_terminal_node.

Must return an EMPTY path, never a fallback step, when the destination is unreachable; finishing through concludeSearch() guarantees this. The path traverses at least one edge, so start_node is never a valid destination.

Parameters:

start_node – The node to start path selection from.
graph – Pointer to the planning graph; nullptr when the layer’s generator is a tree (TreeGenerator).
required_terminal_node – Node the selected path must end at, or nullptr to search freely.

Returns:

The best path and its value, or an empty path when the destination is unreachable.

inline virtual void resetEpisode() override#: Reset all per-episode plugin state.

inline void setLayer(Layer *layer)#: Sets the owning layer; called by the owning layer after setup. getLayer() is nullptr during setupFromParamMap.

virtual void setupFromParamMap(Configurable::ParamMap *param_map) override#

Set up parameters from the parameter map.

“objective” is required unless objective_required_ is false, in which case it defaults to the current p_objective_.

Parameters:: param_map – Pointer to the plugin parameter map.

Protected Functions

std::vector<Node*> concludeSearch(const Path &best, Node *start_node, Node *required_terminal_node, const std::vector<Node*> &fallback_candidates, std::mt19937 *rng, double *score) const#

Turns the running best into the answer.

With nothing selected, an unconstrained search returns a random step to one of fallback_candidates and a constrained one returns the empty path.

Parameters:

best – The running best, as folded by updateBest(); invalid when nothing was selected.
start_node – First node of a fallback step.
required_terminal_node – The destination this search was asked for, or nullptr.
fallback_candidates – Nodes reachable in one step, in a deterministic order.
rng – Engine for the fallback pick; drawn from only when a fallback actually happens.
score – Set to the chosen node’s own gain when a fallback is taken; untouched otherwise.

Throws:

std::runtime_error – when a fallback is needed but fallback_candidates is empty.

Returns:

The selected path, a one-step fallback, or an empty path.

void suppressGains(const GraphType &graph, const Node *start_node) const#

suppressGains() over every vertex of graph.

Parameters:

graph – The graph the search runs on.
start_node – The search’s start node.

void suppressGains(const std::vector<Node*> &nodes, const Node *start_node) const#

Decide Node::is_gain_suppressed for a search from start_node (suppressNonMaximumClusters).

Called at the start of every search, so the flags follow the gains the evaluator holds now.

Parameters:

nodes – Every node the search may reach.
start_node – The search’s start node.

bool updateBest(Path candidate, Path &best, double &best_score, Node *required_terminal_node)#

Fold one candidate into the running best by its Objective score.

Exact ties prefer a frontier end under kExpectedGain/kTableGain, else Path::operator>. kTableGain inserts the candidate into the episodic table before scoring, so the result is order-dependent. Candidates not ending at required_terminal_node are discarded unscored.

Parameters:

candidate – The candidate path to evaluate.
best – Running best path, initially invalid; replaced by candidate when it scores higher.
best_score – Running best score, initially kDoubleMin; updated alongside best.
required_terminal_node – Node the candidate must end at, or nullptr to score them all.

Returns:

Whether candidate became the best.

Protected Attributes

GainCostTable gain_cost_table_#: Episodic optimistic gain-vs-cost frontier, used by the kTableGain objective.

Layer *layer_ = nullptr#

bool objective_required_ = true#: Whether setupFromParamMap() requires the “objective” key; set before setup.

bool p_concave_envelope_ = true#: Whether the gain-cost table uses the optional concave envelope (kTableGain only).

double p_gain_suppression_radius_ = 0.0#: Radius [m] of the non-maximum suppression over node clusters before each search; 0 suppresses nothing.

bool p_is_gain_correlated_ = false#: Whether nodes share gain anchors, so a path counts each anchor once (key is_gain_correlated).

Objective p_objective_ = Objective::kPathGain #: Optimization objective used for path selection.

enum class open_apero::Objective#

Optimization objective for next-best-path selection; C is the agent’s remaining cost budget, as in the paper.

Values:

enumerator kPathGain#: Score: gain.

enumerator kPathRatio#: Score: gain / cost.

enumerator kExpectedGain#: Score: ratio * C when the path ends at a frontier node, gain otherwise.

enumerator kTableGain#: Score: gain + GainCostTable::valueAt(C - cost) at a frontier end, gain otherwise.

inline std::string open_apero::toString(Objective type)#

Convert an Objective enum value to its string representation.

Parameters:: type – The objective to convert.
Throws:: std::invalid_argument – if type is none of the four enumerators.
Returns:: String name of the objective.

inline Objective open_apero::objectiveFromString(const std::string &str)#

Parse the “objective” key into an Objective enum value.

Parameters:: str – String name of the objective.
Throws:: std::runtime_error – if str names none of the four objectives.
Returns:: The corresponding Objective value.

Backtracker#

class Backtracker : public open_apero::Plugin #

#include <open_apero_core/plugin/backtracker/backtracker.h>

Abstract base for backtracking strategies that produce recovery trajectories.

Subclassed by open_apero::backtracker::Reverse, open_apero::backtracker::RotateInPlace

Public Functions

inline explicit Backtracker(AgentInterface &agent)#

virtual bool buildTrajectory(const RobotState &state, RobotTrajectory *out) = 0#

Build a recovery trajectory from the robot’s current state.

Strategy hook implemented by concrete backtrackers (Reverse / RotateInPlace / RotateReverse). Returns false if no recovery is currently possible (e.g. Reverse with an empty queue).

Parameters:

state – Robot’s current state (start of the recovery trajectory).
out – Output trajectory; first waypoint must be at time 0 and size() must be >= 2 on success.

Returns:

true if a valid trajectory was produced.

inline virtual void declareMetrics(MetricsFields&) const#

Registered pointers must outlive the performance log.

inline virtual void getVisualization(VisualizationMarkers*) const#: Append this backtracker’s debug markers for the current recovery state. Default no-op.

inline virtual bool isInitialExplorationDone()#: Whether the backtracker has finished its initial sweep (e.g. a one-time rotation at episode start). The owning layer consults this to decide whether to bump its priority.

inline virtual const std::vector<ProbeSample> &lastProbeSamples() const#: Samples behind the most recent decision. Default: a strategy that keeps none.

inline virtual void onStateReached(const RobotState&)#

Observe the agent’s reached state even when this backtracker does not win this cycle.

The owning BacktrackingLayer calls this from onStateReached so stateful backtrackers can clear per-recovery state once normal planning resumes.

inline virtual bool recordExecutedEdge(const Edge&)#: Record an executed edge for use in future recovery trajectories. Called by the owning layer in onStateReached whenever a non-backtrack edge has just been executed.

inline virtual void resetCycleMetrics()#: Zero the columns declareMetrics registered. Default no-op.

inline virtual void resetEpisode() override#: Resets per-episode backtracker state. Default no-op.

struct ProbeSample#

#include <open_apero_core/plugin/backtracker/backtracker.h>

One candidate a strategy evaluated for its last decision.

Public Members

int branch#: Which alternative of the decision the sample argues for.

bool counted#: Whether the sample contributed to its branch’s count.

Eigen::Vector3d position#: World-frame sensor position of the sample.

int ray#: Direction index within that alternative.

int reason#: 0 traversable; a nonzero value is a strategy-defined reason the sample was rejected or skipped.

int step#: Sample index along the ray.

bool traversable#: Whether the robot is collision-free there.

Plugin Interfaces

Contents

Plugin Interfaces#

Robot Model#

Cost Computer#

Generators#

Evaluators#

Sensor Model#

Path Planner#

Backtracker#