Agent#

The agent owns the map, the robot model, the cost computer and the layer stack, and runs the plan-execute cycle; Agent and Execution explains it. A platform derives from open_apero::Agent, implements sendMovement() and publishVisualization(), and feeds measured states in with setCurrentState(); plugins see the agent only through open_apero::AgentInterface.

class Agent : public open_apero::AgentInterface, public open_apero::Configurable #

#include <open_apero_core/agent/agent.h>

Main agent that drives the active-perception planning loop.

Owns the map, the robot model, the cost computer and the layer stack, which setupFromParamMap() creates through the plugin factory, and runs the plan-execute cycle: loopIteration() returns early until the dispatched edge is complete, then notifies, updates and expands the layers, and requestNextTrajectory() selects and dispatches the next step.

Agent is abstract. A platform derives from it, implements sendMovement() and AgentInterface::publishVisualization(), and feeds measured states in with setCurrentState(). The constructor and setupFromParamMap() are protected, so the subclass constructs and sets itself up: RosAgent::create does both, BenchmarkAgent is a minimal driver without a robot.

Subclassed by open_apero::BenchmarkAgent, open_apero::ros::RosAgent

Public Types

enum class CommandPhase#

Where the command sent this cycle stands. The map-update gate may wait for a mapped frame only once a command was published (any phase but kNone): a lockstep simulator produces frames only in response to commands.

Values:

enumerator kNone#: No command sent since the map-update gate was armed (startup, veto, episode end); no frame can come.

enumerator kPending#: Command published; its resulting state has not arrived yet.

enumerator kResultHeld#: The state currently held is the command’s own result. Lasts until the next state arrives: an asynchronous source (TF poll, pose stream) also delivers samples that are merely periodic, and only in this phase may a consumer judge what the command achieved.

enumerator kResolved#: Result consumed; a map frame for the command may still arrive.

enum class EpisodeEnd#

How an episode segment ended; episode_summary.yaml records it as end_reason.

Values:

enumerator kBudgetExhausted#

enumerator kGoalReached#

enumerator kNoStep#

enumerator kStopped#

enum class ExecutionStatus#

Why the most recent execution step did or did not dispatch a trajectory. Set at each decision point in executeTrajectory/hasSettled, so a non-ROS driver can tell waiting from planning or termination.

Values:

enumerator kAwaitingCommandResult#: A command is in flight; its resulting state has not arrived.

enumerator kAwaitingPose#: Reported pose is not yet within thresholds of the target waypoint.

enumerator kAwaitingMapUpdate#: Pose reached, but no matching optical map frame has arrived yet.

enumerator kPlanned#: A trajectory was dispatched this step.

enumerator kTerminated#: The episode ended (goal reached, budget exhausted, or no step offered).

enum class State#

Lifecycle of the agent’s episode.

kIdle: no episode (constructed, or reset before a state arrived). kAwaitingInitialState: set only by platforms that wait for the first pose before planning (RosAgent). kPlanning: the episode runs and planningLoop() calls loopIteration(). kTerminated: the episode ended (terminateEpisode()) and planningLoop() returns.

Values:

enumerator kIdle#

enumerator kAwaitingInitialState#

enumerator kPlanning#

enumerator kTerminated#

enum class StateUpdateKind#

Whether an incoming state completes a command or merely observes motion.

Values:

enumerator kCommandResult#: Authoritative state produced after the pending command completed.

enumerator kPoseObservation#: Intermediate/asynchronous pose; must not resolve the pending command.

Public Functions

bool configureRunOutput(const RunOutputConfig &config)#

Activates a run’s output directory: run_manifest.yaml always, performance_log.csv with log_performance, map_log.csv and maps/ with num_maps_to_save > 0. All-or-nothing: on failure only files this call created are removed, and existing target files are rejected. A failed attempt may be retried. Idempotent for the same directory; a different one is rejected.

Returns:: True if the run is configured (now or already, same directory).

inline ExecutionStatus getExecutionStatus() const#: Status of the most recent execution step (see ExecutionStatus).

inline AgentMetrics &getMetrics()#

inline bool loggingRequested() const#: Whether this run produces any on-disk artifacts that need a directory.

virtual void planningLoop()#: Runs loopIteration() while planning until the episode terminates; in any other state it sleeps 1 ms per pass.

inline void reduceCostBudget(double cost_to_reduce)#: Charges cost_to_reduce to the remaining budget; the agent does so for every executed edge.

void reportUnreadParams(const Configurable::ParamMap &param_map) const#: Warns about, or with strict_params throws for, top-level keys of the agent’s map that nothing read. Platforms call it after their complete setup (RosAgent::create does).

virtual void resetEpisode()#

Starts a new episode: clearEpisodeState() to drop what the finished episode accumulated, then onPlanningStart() to root the graph in the state currently held.

Precondition: a state is held (hasCurrentState()); onPlanningStart() dereferences it. RosAgent’s override also handles the case without one.

void resumeEpisode()#: Re-arms finalizeEpisodeOutput() for an episode resumed after a stop, so its real end writes the run output again.

inline virtual void saveScreenshot()#: Saves a screenshot; called when the budget is exhausted and by RosAgent’s stop. No-op by default.

inline void setCostBudget(double cost_budget)#: Sets both the configured and the remaining budget to cost_budget, overriding the cost_budget parameter.

Public Static Functions

static inline bool isAwaitingExternalInput(ExecutionStatus status)#: True while execution waits on a command result, a pose within thresholds or a mapped frame. All arrive through callbacks, so a driver may back off meanwhile.

Protected Functions

Agent(PluginFactory *factory, Configurable::ParamMap *param_map)#

Parameters:

factory – Factory the agent creates its plugins with; must outlive the agent.
param_map – Not read by the constructor. The creator passes the same map to setupFromParamMap() once the object is complete (RosAgent::create); a caller that never sets up (BenchmarkAgent) may pass nullptr.

void clearEpisodeState()#: Drops everything the finished episode accumulated – map, robot, cost computer, history, metrics, layers, and the dispatch the finished cycle left behind – without needing a pose. onPlanningStart() is the half that needs the pose; resetEpisode() is the two in sequence.

bool executeTrajectory()#

Advances execution of the dispatched edge: once the robot has settled on the commanded waypoint, sends the next one through sendMovement().

Returns:: True once the whole edge is complete, or when nothing is dispatched; false while waiting for a command result, the pose or a map frame, or right after sending the next waypoint.

void finalizeEpisodeOutput(EpisodeEnd end)#: Writes episode_summary.yaml (with the run output configured) and, with an unbounded budget, the final map snapshot. Once per episode segment: termination and a platform’s stop both call it; resumeEpisode() re-arms it.

bool hasSettled(const RobotState &target, bool log_on_block = false)#

Whether the robot has settled on target: pose within the target thresholds and, with wait_for_map_updated, a mapped frame near it.

Parameters:

target – Waypoint the robot was commanded to.
log_on_block – When true, emit a diagnostic naming the gate (pose vs map-update) that is keeping the robot from settling; executeTrajectory() sets it for a wait over a second, at most once a second.

Returns:

True if the robot may proceed past target.

virtual void loopIteration()#

One pass of planningLoop(): the first half of a cycle.

Returns early until executeTrajectory() reports the dispatched edge complete. Then records the edge, calls Layer::onStateReached() on every layer, charges the edge to the budget (terminating the episode when the budget is spent or the goal is reached), updates the robot model and the layers, expands the layers when readyToExpand(), and calls requestNextTrajectory().

inline virtual void onGoalReached()#: Called once when a terminating edge has been executed to its end; the platform stops the robot and records the goal.

virtual void onPlanningStart()#

Roots a new episode in the state currently held: records it as the initial state, restores the full cost budget, enters State::kPlanning and calls Layer::onEpisodeStart() on every layer.

Precondition: a state is held (hasCurrentState()).

inline virtual bool readyToExpand()#: Whether the agent should expand each layer this iteration. Default returns true; subclasses may gate (e.g. on auxiliary state).

void recordObservedPoses()#: Copies the frames the map reports as integrated into the observation history. Call once per cycle; the map keeps them until the next dispatch clears them.

virtual bool requestNextTrajectory()#

Selects and dispatches the next step: the second half of a cycle, after loopIteration() has notified, updated and expanded the layers.

Snapshots every layer’s getPriority(), asks the layers in descending priority for planPath() until one offers a step, publishes the visualization, writes the performance-log row (with log_performance), arms the map-update gate and dispatches the step’s first edge through sendMovement().

Throws:: std::runtime_error – if two layers report the same runtime priority, or the winning step has fewer than two nodes, no edge, or a first edge with fewer than two states or not starting at time 0.
Returns:: False when no layer offers a step; the episode then terminates (EpisodeEnd::kNoStep).

inline virtual void respondToHit(bool hit_really_happened)#

Hook for subclasses that detect a collision or a vetoed step; nothing in the core calls it. The default does nothing.

Parameters:: hit_really_happened – True when the robot was observed failing to reach the commanded waypoint, false when the remaining trajectory was vetoed before dispatch.

void saveMapSnapshot()#: Saves map snapshot current_map_index_ to maps/ and appends its map_log.csv row. Precondition: num_maps_to_save > 0 and the run output is configured.

virtual bool sendMovement(EdgeCursor *cursor) = 0#

Sends the edge under cursor to the robot: the hook through which every platform moves.

Called with the selected edge at waypoint_idx 0 and, until cursor->isFinished(), again each time the robot has settled on cursor->to(). Advance the cursor past what was commanded: EdgeCursor::finish() when the whole trajectory is sent (RosAgent, BenchmarkAgent), EdgeCursor::advance() for one waypoint. A cursor left at 0 makes the agent settle on the start state and call this again.

Parameters:: cursor – The active edge and waypoint index; never null.
Returns:: True if a command was published; execution then waits for its result, which the platform delivers with setCurrentState(state, StateUpdateKind::kCommandResult). False if nothing was published (e.g. a pre-flight veto): no result is awaited and, on an edge’s first dispatch, the map-update gate expects no frame (CommandPhase::kNone).

void setCurrentState(const RobotState &robot_state, StateUpdateKind update_kind)#

Store the latest robot state after applying agent-level state normalization.

Parameters:

robot_state – State to store.
update_kind – Whether this state completes the pending command.

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

Reads the agent parameters and creates the map, robot, cost computer and layer stack.

Parameters:: param_map – Agent parameters; nullptr leaves every parameter at its default and creates nothing.

void terminateEpisode(EpisodeEnd end)#: Ends the episode: nothing plans or is dispatched. Silent; each call site logs its own reason. The first call since the last reset or resume finalizes the run output with end.

void updateFrameLedger()#: Sets AgentMetrics::frames_mapped to the number of frames the map reports integrating since the last dispatch, and logs an error when it is below commands_dispatched. A map that reports no frame list counts 0 and is reported, as a warning, only with debug_diagnostics.

Protected Static Functions

static void zeroSensorRoll(RobotState *robot_state)#: Zeroes the roll of a state’s sensor orientation in place (the sensor is assumed level). Afterwards getSensorYaw and getSensorEulerAnglesZyx()(0) agree exactly, unless the sensor points straight up or down (gimbal lock).

open_apero::BenchmarkAgent runs the planner without a robot or ROS: movement finishes at once. The graph benchmark drives the path planners through it.

class BenchmarkAgent : public open_apero::Agent #

#include <open_apero_core/agent/benchmark_agent.h>

Agent stub that runs without a live robot, so path-planner benchmarks can drive the graph search without hardware.

Overrides sendMovement with a no-op that immediately finishes the cursor. A caller that wants markers, as the graph benchmark node does, publishes them itself; the in-class publishVisualization is a no-op.

Public Functions

inline BenchmarkAgent(PluginFactory *factory, Configurable::ParamMap *param_map)#

Construct a BenchmarkAgent.

Parameters:

factory – Plugin factory; the benchmark passes an empty PluginFactoryConfig, since it builds the planners itself and configures them through setupRaw().
param_map – Parameter map (may be nullptr for raw benchmarks).

inline virtual void publishVisualization(const VisualizationMarkers &markers) override#

No-op visualization; markers are discarded.

Parameters:: markers – Visualization markers (ignored).

inline void setRandomSeed(int random_seed)#

Set the episode random seed.

Benchmarks construct the agent with a null parameter map, so setupFromParamMap() never runs and the seed would otherwise keep its nondeterministic default (-1). Path planners pull this through AgentInterface::getRandomSeed() when seeding their tie-break RNGs.

Parameters:: random_seed – Episode seed; negative means nondeterministic.

~BenchmarkAgent() override = default#

Protected Functions

inline virtual bool sendMovement(EdgeCursor *cursor) override#

No-op movement send; simulates a successful execution by immediately finishing the cursor.

Parameters:: cursor – The edge cursor containing the edge to execute.
Returns:: Always true.

class AgentInterface#

#include <open_apero_core/agent/agent_interface.h>

Abstract interface that plugins use to access agent state and services.

Provides accessors for robot position, map, plugins, and utility methods for visualization and logging.

Subclassed by open_apero::Agent

Public Functions

inline virtual CostComputer *getCostComputer()#: The agent’s edge-cost metric, shared by every layer. Agent always configures one; nullptr when an implementation has none.

virtual const Eigen::Vector3d &getCurrentBasePosition() const = 0#: Returns the robot’s current base position in the world frame.

virtual double getCurrentCostBudget() const = 0#: The budget left: the configured budget minus the cost of every edge executed this episode. Path planners prune candidate paths that would exceed it.

virtual const Eigen::Vector3d &getCurrentSensorPosition() const = 0#: Returns the robot’s current sensor position in the world frame.

virtual PluginFactory &getFactory() = 0#: Returns a reference to the plugin factory.

virtual HistoryData &getHistoryData() = 0#: Returns a reference to the agent’s history data.

virtual const Eigen::Vector3d &getInitialSensorPosition() const = 0#: Returns the sensor position at the start of the planning session.

virtual Map &getMap() = 0#: Returns a reference to the map.

virtual double getOriginalCostBudget() const = 0#: The episode’s configured cost budget, shared by all layers: the agent parameter cost_budget (+inf when unbounded) unless Agent::setCostBudget() overrode it. It does not shrink as the episode spends; see getCurrentCostBudget().

inline virtual int getRandomSeed() const#: Episode-wide random seed, or -1 for nondeterministic behavior. Stochastic plugins seed their own engine from this so a run reproduces.

virtual RobotModel &getRobot() = 0#: Returns a reference to the robot model.

virtual double getUsedCostBudget() const = 0#: The budget spent so far: getOriginalCostBudget() - getCurrentCostBudget().

inline virtual bool hasCurrentState() const#: Whether a current robot state is held. Between episodes there is none and the position getters throw, so callbacks that can fire then check this first.

virtual bool isCostBudgetPositive() const = 0#: Whether any budget is left; the agent ends the episode once it is not.

inline virtual bool isDebugDiagnosticsEnabled() const#

Whether high-detail troubleshooting diagnostics are enabled.

Defaults off so external and test AgentInterface implementations do not need to opt into potentially noisy terminal output.

virtual bool isLoggingPerformance() const = 0#: Whether the agent is writing a per-cycle CSV log. Used by layers to decide whether to arm their ScopedTimer instances.

virtual void printError(const std::string &text) = 0#

Logs an error message.

Parameters:: text – The message text.

virtual void printInfo(const std::string &text) = 0#

Logs an informational message.

Parameters:: text – The message text.

virtual void printWarning(const std::string &text) = 0#

Logs a warning message.

Parameters:: text – The message text.

inline virtual void publishNamedVisualization(const std::string &name, const VisualizationMarkers &markers)#

Publishes one named group of visualization markers to its own output (e.g., a dedicated RViz MarkerArray topic) so each concern – a planner layer, the map, the robot footprint, etc. – can be toggled independently. Default no-op; ROS agents override.

Parameters:

name – Topic-relative name identifying this group of markers.
markers – The markers for this group.

virtual void publishVisualization(const VisualizationMarkers &markers) = 0#

Publishes visualization markers to the output layer (e.g., RViz).

Parameters:: markers – The markers to publish.

virtual ~AgentInterface() = default#

struct EdgeCursor#

#include <open_apero_core/graph/edge_cursor.h>

Cursor holding the active edge and current waypoint index. Used so all trajectory-progress operations are in one place.

The agent creates one per dispatched edge and hands it to Agent::sendMovement(), which advances it past the waypoints it commanded.

Public Functions

inline void advance()#: Moves to the next waypoint: one more waypoint has been commanded.

inline explicit EdgeCursor(Edge *e)#

Starts at waypoint 0 of e.

Throws:: std::invalid_argument – if e is null.

inline void finish()#: Jumps to the last waypoint: the whole remaining trajectory has been commanded.

inline const RobotState &from() const#: Previous waypoint (where the robot currently is). Precondition: waypoint_idx >= 1.

inline RobotTrajectory getTailTrajectory() const#: Returns the remaining trajectory from the current waypoint_idx.

inline bool isFinished() const#: True once waypoint_idx has reached the edge’s last state.

inline const RobotState &to() const#: Next waypoint (where the robot is heading).

Public Members

Edge *edge = nullptr#: The edge being executed; not owned, never null.

int waypoint_idx = 0#: Index of the waypoint the robot is heading to: 0 (the edge’s start state) until sendMovement() advances the cursor.

struct RunOutputConfig#

#include <open_apero_core/agent/agent.h>

Where a run’s output artifacts go.

Analysis-only inputs (e.g. a plot’s reference line) do not belong here; the manifest records only what the run observes.

Public Members

std::string run_dir#: Directory to write the run’s artifacts into.

Agent

Contents

Agent#