Map

Map#

The map interface the agent, the robot model and the evaluators query, the contracts that refine it, and the Voxblox backend; Maps explains the contracts.

class Map : public open_apero::Plugin#
#include <open_apero_core/plugin/map/map.h>

Base class of every map backend used by the agent.

Declares no pure virtual method: every method has a default (no-op, false or 0), so a backend overrides only what it supports. Voxel occupancy queries, which SensorModel and the evaluators use, are declared by map::OccupancyMap; distance queries, which SimpleRobot’s traversability check uses, by map::TsdfMap.

Subclassed by open_apero::map::OccupancyMap

Public Functions

inline virtual std::vector<std::string> agentParamsReadByBackend() const#

Keys of the agent’s namespace that the backend reads itself, not through setParam(), such as the parameters of an embedded mapping server; the platform agent counts them as read. Default: none.

inline virtual double getExploredVolume() const#

Returns the total volume that has been explored.

Returns:

Explored volume in cubic meters.

virtual bool getGroundHeight(const RobotState &state, double *ground_z) const#

Reports the height of the traversable ground under a state, if known.

Default: false (no ground model).

Parameters:

  • state – Robot state whose ground to look up.

  • ground_z – Output ground height in world coordinates; unchanged on failure.

Returns:

true if a ground height was resolved for this state.

virtual void getHitRegions(GeometryPrimitiveCollection &hit_regions) const#

Retrieves all recorded obstacle hit region primitives.

Parameters:

hit_regions – Output collection of hit region geometry primitives.

virtual bool getMappedOpticalFrameTransforms(std::vector<Transform3d> *transforms)#

Gets the optical-frame transforms the map integrated since the last resetMapUpdateStatus().

Parameters:

transforms – Output list of optical frame transforms.

Returns:

true if transforms are available.

inline virtual double getMappedSurfaceArea() const#

Returns the total mapped surface area in square meters.

virtual bool getTotalMapRegion(Eigen::Vector3d *total_map_min, Eigen::Vector3d *total_map_max)#

Gets the axis-aligned bounding box encompassing the entire map.

Parameters:

  • total_map_min – Output minimum corner.

  • total_map_max – Output maximum corner.

Returns:

true if the map region is available.

virtual bool getUpdatedMapRegion(Eigen::Vector3d *updated_map_min, Eigen::Vector3d *updated_map_max)#

Gets the axis-aligned bounding box of the map region updated since the last resetMapUpdateStatus().

Parameters:

  • updated_map_min – Output minimum corner of the updated region.

  • updated_map_max – Output maximum corner of the updated region.

Returns:

true if an update region is available.

virtual void getVisualization(VisualizationMarkers *markers)#

Generates visualization markers for the current map state.

Produces hit-region triangles, mapped-optical-frame arrows, and bounding-box cubes for the recently updated and total map extents.

Parameters:

markers – Output marker container.

virtual bool hasMappedFrameNear(const Transform3d &optical_frame, double position_tolerance, double angle_tolerance)#

Reports whether an integrated frame lies within the tolerances of an optical frame.

Default: scans getMappedOpticalFrameTransforms. Backends whose readiness needs more than the geometric frame list override this.

Parameters:

  • optical_frame – Optical-frame transform to match.

  • position_tolerance – Position tolerance in meters.

  • angle_tolerance – Orientation tolerance in radians.

Returns:

true if a frame integrated since the last resetMapUpdateStatus() matches.

virtual bool isStateReachable(const RobotState &state) const#

Reports whether the exact cell of a state is currently reachable.

Default: true (no information). Backends with a walkable-surface model override it, e.g. to reject states inside a registered hit region.

Parameters:

state – Robot state to test.

Returns:

true if the state’s cell is reachable (or unknown).

explicit Map(AgentInterface &agent)#

Constructs a Map and binds it to the agent.

virtual void registerHit(const RobotState &stopped, const RobotState &target)#

Marks a failed motion (stopped -> target) as untraversable.

Default: no-op. Backends that override it may also block the stopped state’s own region, so recovery planners must handle escaping from a blocked start; blocked regions accumulate until resetEpisode().

Parameters:

  • stopped – The state where the robot actually stopped.

  • target – The target/planned state the robot failed to reach.

inline virtual void resetEpisode() override#

Resets per-episode map state (accumulated data, update tracking). Default: no-op.

inline virtual bool resetMapUpdateStatus()#

Starts a new map-update window.

The agent calls it once per cycle, right before dispatching the next step (and when no layer offers one). A backend clears the updated region getUpdatedMapRegion() reports and the frames getMappedOpticalFrameTransforms() and hasMappedFrameNear() report, so with wait_for_map_updated they then cover only frames integrated since that dispatch. The default does nothing.

Returns:

Whether the backend reset its status; the agent ignores it. The default returns false.

virtual bool saveMap(const std::string &filename)#

Persists the current map to a file.

Parameters:

filename – Path to the output map file.

Returns:

true if the map was saved successfully.

virtual void setHitRegions(const GeometryPrimitiveCollection &hit_regions)#

Replaces the stored obstacle hit region primitives.

Parameters:

hit_regions – Hit region geometry primitives to store.

virtual ~Map() = default#
class OccupancyMap : public open_apero::Map#
#include <open_apero_core/plugin/map/occupancy_map.h>

Abstract base class for occupancy grid map implementations.

Provides voxel-level occupancy queries on top of the Map interface.

Subclassed by open_apero::map::TsdfMap

Public Functions

inline virtual bool getSurfaceVoxelCenter(Eigen::Vector3d *center, const Eigen::Vector3d &sample, const Eigen::Vector3d &direction)#

Returns the center of the voxel holding the surface a ray sample reads kOccupied from.

The default is the sample’s own voxel.

Parameters:

  • center – Output voxel center in world frame.

  • sample – Ray sample that reads kOccupied.

  • direction – Unit direction of the ray.

Returns:

true if the voxel center was successfully computed.

virtual bool getVoxelCenter(Eigen::Vector3d *center, const Eigen::Vector3d &point) = 0#

Returns the center position of the voxel containing the given point.

Parameters:

  • center – Output voxel center in world frame.

  • point – Query point in world frame.

Returns:

true if the voxel center was successfully computed.

virtual Occupancy getVoxelOccupancy(const Eigen::Vector3d &point) = 0#

Returns the occupancy state of the voxel containing the given point.

Parameters:

point – Query point in world frame.

Returns:

Occupancy state of the voxel.

virtual double getVoxelSize() = 0#

Returns the side length of a single voxel.

Returns:

Voxel size in meters.

inline explicit OccupancyMap(AgentInterface &agent)#

Constructs an OccupancyMap and binds it to the agent.

virtual ~OccupancyMap() = default#
class TsdfMap : public open_apero::map::OccupancyMap#
#include <open_apero_core/plugin/map/tsdf_map.h>

Abstract base class for TSDF voxel grid map implementations.

Adds signed distance and observation weight queries to the occupancy map interface.

Subclassed by open_apero::map::VoxbloxMap

Public Functions

virtual bool getDistanceAtPosition(const Eigen::Vector3d &position, double *distance) = 0#

Returns the ESDF/distance-field value at the given position.

Parameters:

  • position – Query position in world frame.

  • distance – Output distance to the nearest obstacle in meters.

Returns:

true if a distance value is available.

virtual double getMaximumWeight() = 0#

Returns the maximum allowed observation weight, or 0 if weights are uncapped (VoxbloxMap returns the integrator’s max_weight).

Returns:

Maximum weight value.

inline virtual CollisionStatus getSphereCollisionStatus(const Eigen::Vector3d &position, double radius)#

Checks whether a world-frame sphere collides with the distance field.

Parameters:

  • position – Sphere center in world coordinates.

  • radius – Sphere radius in meters.

Returns:

kColliding, kNotColliding, or kUnknown if the map has no distance value.

inline virtual bool getSurfaceVoxelCenter(Eigen::Vector3d *center, const Eigen::Vector3d &sample, const Eigen::Vector3d &direction) override#

Returns the center of the voxel holding the surface a ray sample reads kOccupied from.

Of the voxels one voxel before, at and one voxel past the sample along the ray, the one whose center is closest to the surface. The sample’s own voxel when none has a distance value.

Parameters:

  • center – Output voxel center in world frame.

  • sample – Ray sample that reads kOccupied.

  • direction – Unit direction of the ray.

Returns:

true if the voxel center was successfully computed.

virtual double getVoxelWeight(const Eigen::Vector3d &point) = 0#

Returns the observation weight stored at the voxel containing the given point.

Parameters:

point – Query point in world frame.

Returns:

Observation weight (higher means more confident).

inline explicit TsdfMap(AgentInterface &agent)#

Constructs a TsdfMap and binds it to the agent.

virtual ~TsdfMap() = default#

Voxblox Backend#

From the open_apero_voxblox package.

class VoxbloxMap : public open_apero::map::TsdfMap#
#include <open_apero_voxblox/plugin/map/voxblox_map.h>

TSDF/ESDF map backed by voxblox, providing collision checking, occupancy queries, distance fields, and volumetric metrics.

Public Functions

virtual std::vector<std::string> agentParamsReadByBackend() const override#

nodeParamsReadByVoxblox(): the embedded servers read the agent’s (the planner node’s) namespace.

virtual bool getDistanceAtPosition(const Eigen::Vector3d &position, double *distance) override#

Returns the ESDF distance at the given position.

Parameters:

  • position – Query position in world coordinates.

  • distance – Output ESDF distance value.

Returns:

True if the distance was successfully retrieved.

voxblox::EsdfServer &getEsdfServer()#

Provide direct access to the underlying voxblox ESDF server.

virtual double getExploredVolume() const override#

Compute the total observed volume in cubic meters.

virtual bool getMappedOpticalFrameTransforms(std::vector<Transform3d> *transforms) override#

Gets optical-frame transforms integrated since the last reset.

Parameters:

transforms – Output optical frame transforms.

Returns:

True if transforms are available.

virtual double getMappedSurfaceArea() const override#

Compute the mapped surface area in square meters.

virtual double getMaximumWeight() override#

Return the maximum allowed TSDF weight (0 if uncapped).

virtual bool getTotalMapRegion(Eigen::Vector3d *total_map_min, Eigen::Vector3d *total_map_max) override#

Get the axis-aligned bounding box of the entire map.

Parameters:

  • total_map_min – Output minimum corner.

  • total_map_max – Output maximum corner.

Returns:

True if the map is non-empty.

virtual bool getUpdatedMapRegion(Eigen::Vector3d *updated_map_min, Eigen::Vector3d *updated_map_max) override#

Get the axis-aligned bounding box of recently updated map regions.

Parameters:

  • updated_map_min – Output minimum corner.

  • updated_map_max – Output maximum corner.

Returns:

True if an updated region exists.

virtual bool getVoxelCenter(Eigen::Vector3d *center, const Eigen::Vector3d &point) override#

Compute the center coordinates of the voxel containing a point.

Parameters:

  • center – Output voxel center position.

  • point – The input query point.

Returns:

True on success.

virtual Occupancy getVoxelOccupancy(const Eigen::Vector3d &point) override#

Get the occupancy status of a voxel.

Parameters:

point – The 3D query point.

Returns:

kOccupied, kFree, or kUnknown.

virtual double getVoxelSize() override#

Return the voxel side length.

virtual double getVoxelWeight(const Eigen::Vector3d &point) override#

Get the stored TSDF integration weight at a point.

Parameters:

point – The 3D query point.

Returns:

The weight value, or 0.0 if not found.

virtual void resetEpisode() override#

Resets all voxblox map data (TSDF + ESDF layers) for a new episode.

virtual bool resetMapUpdateStatus() override#

Clear the map-update tracking state.

virtual bool saveMap(const std::string &filename) override#

Persist the voxblox map to disk via the underlying ESDF server.

Parameters:

filename – Path to the output map file.

Returns:

True on success.

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

Initialize the ESDF server and cache map constants from the param map.

Parameters:

param_map – Pointer to the parameter map.

explicit VoxbloxMap(AgentInterface &agent)#

Construct the VoxbloxMap.

Parameters:

agent – Reference to the parent agent interface.

Public Static Functions

static const std::vector<std::string> &nodeParamsReadByVoxblox()#

Every parameter voxblox_ros reads from a node’s private namespace, sorted (C locale): a superset of what the embedded servers read. They share the planner node’s namespace, so the agent must not report them.

Enumerations#

enum class open_apero::Occupancy#

Voxel occupancy state.

Values:

enumerator kFree#
enumerator kOccupied#
enumerator kUnknown#
enum class open_apero::CollisionStatus#

Result of a collision check.

Values:

enumerator kColliding#
enumerator kNotColliding#
enumerator kUnknown#

Hit Regions#

struct GeometryPrimitiveCollection#
#include <open_apero_core/geometry/geometry_primitives.h>

A collection of geometry primitives used to represent hit/blocked regions.

Public Functions

void append(const GeometryPrimitiveCollection &other)#

Append all primitives from another collection into this one.

Parameters:

other – The collection to append.

void clear()#

Clear all primitives from the collection.

bool getBounds(Eigen::Vector3d *min, Eigen::Vector3d *max) const#

Axis-aligned bounds of every primitive in the collection.

Parameters:

  • min – Output minimum corner.

  • max – Output maximum corner.

Returns:

false, with min and max unchanged, when the collection is empty.

Public Members

std::vector<Cylinder> cylinders#

Collection of cylinder primitives.

std::vector<Sphere> spheres#

Collection of sphere primitives.

std::vector<TriangularPrism> triangles#

Collection of triangular prism primitives (named for their triangular bases).