ROS Integration

ROS Integration#

The ROS 1 agent, its plugin factory and the message conversions (open_apero_ros, open_apero_ros_bridge); ROS Node, Launch Files and Messages explains them. The ROS-free BenchmarkAgent is on Agent.

ROS Agent and Conversions#

namespace ros#

Functions

inline jsk_rviz_plugins::OverlayText buildStatusOverlay(const std::vector<std::string> &lines)#

The status overlay showing lines top to bottom, at the top-left of the view.

The plugin draws the text as Qt rich text, so the lines go out as one preformatted block: spacing is kept as written and markup characters are escaped. The box is sized from DejaVu Sans Mono’s metrics, so a line never wraps.

Parameters:

lines – Plain text; columns padded with spaces line up, the font is monospaced.

Returns:

The message to publish.

inline void quaternionEigenFromMsg(const geometry_msgs::Quaternion &msg, Eigen::Quaterniond *eigen)#

Convert a geometry_msgs::Quaternion to an Eigen quaternion.

Parameters:

  • msg – Input ROS Quaternion message.

  • eigen – Output Eigen quaternion.

inline void quaternionEigenToMsg(const Eigen::Quaterniond &eigen, geometry_msgs::Quaternion *msg)#

Convert an Eigen quaternion to a geometry_msgs::Quaternion.

Parameters:

  • eigen – Input Eigen quaternion.

  • msg – Output ROS Quaternion message.

inline bool robotStateFromMsg(const geometry_msgs::Transform &base_transform, const geometry_msgs::Transform &sensor_transform, RobotState *robot_state)#

Construct an internal RobotState from separate base and sensor TF transforms.

Same orientation validation contract as the message overload above.

Parameters:

  • base_transform – Base link transform in map frame.

  • sensor_transform – Sensor (camera) transform in map frame.

  • robot_state – Output robot state; unspecified contents on failure.

Returns:

False if either quaternion is non-finite or not unit-norm.

inline bool robotStateFromMsg(const open_apero_msgs::RobotState &msg, RobotState *robot_state)#

Convert a ROS RobotState message to an internal RobotState.

Orientations are validated on the way in: every correct publisher sends unit quaternions, so a non-unit one is that publisher’s bug — rejected here with the message, never silently normalized into something plausible.

Parameters:

  • msg – Input ROS RobotState message.

  • robot_state – Output robot state; unspecified contents on failure.

Returns:

False if either quaternion is non-finite or not unit-norm.

inline void robotStateToMsg(const RobotState &robot_state, open_apero_msgs::RobotState *msg)#

Convert an internal RobotState to its ROS message representation.

Parameters:

  • robot_state – Input robot state.

  • msg – Output ROS RobotState message.

inline void vectorEigenFromMsg(const geometry_msgs::Point &msg, Eigen::Vector3d *eigen)#

Convert a geometry_msgs::Point to an Eigen 3D vector.

Parameters:

  • msg – Input ROS Point message.

  • eigen – Output Eigen vector.

inline void vectorEigenFromMsg(const geometry_msgs::Vector3 &msg, Eigen::Vector3d *eigen)#

Convert a geometry_msgs::Vector3 to an Eigen 3D vector.

Parameters:

  • msg – Input ROS Vector3 message.

  • eigen – Output Eigen vector.

inline void vectorEigenToMsg(const Eigen::Vector3d &eigen, geometry_msgs::Point *msg)#

Convert an Eigen 3D vector to a geometry_msgs::Point.

Parameters:

  • eigen – Input Eigen vector.

  • msg – Output ROS Point message.

inline void vectorEigenToMsg(const Eigen::Vector3d &eigen, geometry_msgs::Vector3 *msg)#

Convert an Eigen 3D vector to a geometry_msgs::Vector3.

Parameters:

  • eigen – Input Eigen vector.

  • msg – Output ROS Vector3 message.

inline void visualizationMarkersToMsg(const VisualizationMarkers &markers, visualization_msgs::MarkerArray *msg)#

Append a collection of internal VisualizationMarkers to msg->markers.

An ADD of a point-list marker without points is left out: RViz rejects it, and the error marks the whole display.

Parameters:

  • markers – Input visualization markers collection.

  • msg – ROS MarkerArray message whose existing markers are kept.

inline void visualizationMarkerToMsg(const VisualizationMarker &marker, visualization_msgs::Marker *msg)#

Convert a single internal VisualizationMarker to a ROS Marker message.

Parameters:

  • marker – Input visualization marker.

  • msg – Output ROS Marker message.

class PluginFactoryRos : public open_apero::PluginFactory#
#include <open_apero_ros/plugin/plugin_factory_ros.h>

Concrete plugin factory that reads plugin parameters from the ROS parameter server.

Extends PluginFactory to resolve plugin types and parameter maps from the ROS parameter server namespace hierarchy.

Public Functions

virtual std::vector<std::string> listChildNamespaces(const std::string &ns) override#

List the immediate child sub-namespaces of ns on the ROS parameter server.

Reads ns as a single XmlRpc struct and returns its member names. Throws if ns is present but is not a dictionary, or if any direct member is not itself a dictionary (so a stray scalar next to valid layer sections is a hard error rather than being silently skipped). Returns an empty list when ns is absent.

Parameters:

ns – Absolute ROS parameter namespace to enumerate.

Returns:

Member names, sorted (XmlRpc structs are map-backed).

PluginFactoryRos()#

Default constructor.

~PluginFactoryRos() = default#

Public Static Functions

static std::vector<std::string> leafParamKeys(const std::vector<std::string> &names, std::string ns)#

Select the leaf parameters directly inside a namespace.

Parameters:

  • names – Absolute parameter names, as ros::NodeHandle::getParamNames() lists them.

  • ns – Absolute namespace; a trailing ‘/’ is ignored.

Returns:

The names below ns + "/" with no further ‘/’, relative to it.

class RosAgent : public open_apero::Agent#
#include <open_apero_ros/agent/ros_agent.h>

ROS-based online exploration agent.

Wraps the core Agent with ROS subscribers, publishers, services, and TF-based pose tracking so that exploration can run on a live robot or in simulation via ROS topics.

Public Functions

void clearNamedVisualizations()#

Clear every known named visualization topic (publishes a lone DELETEALL to each). resetEpisode() calls it so the previous episode’s markers do not linger during the idle gap.

void commandResultCallback(const open_apero_msgs::CommandResult &msg)#

Accept the explicitly completed state for the pending TF-mode command.

Parameters:

msg – Stamped final state carrying the matching command sequence.

virtual void planningLoop() override#

Main planning loop; spins ROS and calls loopIteration().

Returns when ROS shuts down, or when an episode terminates if exit_on_episode_end is set. With that parameter false the loop keeps spinning after termination so the reset service can start the next episode. A shutdown during a started episode finalizes its run output as EpisodeEnd::kStopped.

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

Publish one named group of markers to its own RViz MarkerArray topic, creating the publisher on first use. Publishes nothing without subscribers; otherwise a leading DELETEALL (even for an empty set) clears the group’s previous markers.

Parameters:

  • name – Topic name relative to the node’s namespace, not the private one: /name for open_apero_node (e.g. map_visualization, layer_visualization/planning).

  • markers – The markers for this group.

virtual void publishVisualization(const VisualizationMarkers &markers) override#

Publish visualization markers and status overlay to RViz.

Parameters:

markers – The set of visualization markers to publish.

virtual void resetEpisode() override#

Resets all agent state for a new episode, including ROS-specific state.

Extends Agent::resetEpisode() by also resetting ROS timing counters and clearing the named visualization topics. With a pose held it resets fully; without one it clears the episode state immediately and roots the graph once the first pose arrives. Can be triggered via the ~/reset ROS service.

bool resetSrvCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)#

Service callback to reset the agent for a new episode.

Parameters:

  • req – Empty request.

  • res – Empty response.

Returns:

True on success.

void robotStateCallback(const open_apero_msgs::RobotState &msg)#

ROS callback for incoming robot state messages.

Parameters:

msg – The robot state message containing base and sensor transforms.

bool runSrvCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)#

Service callback to toggle planning on or off.

Parameters:

  • req – Request with a boolean data field (true = start, or resume an episode a stop interrupted, refused once the episode has ended; false = stop, which finalizes the run output of a started episode).

  • res – Response indicating success.

Returns:

True on success.

virtual void saveScreenshot() override#

Save an RViz screenshot to the log directory; warns when RViz does not answer.

void updatePoseFromTF(const ::ros::TimerEvent &event)#

Timer callback that updates the robot pose from TF.

Parameters:

event – The timer event (unused beyond triggering the callback).

bool wallTimeSrvCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)#

Service callback that returns the wall-clock seconds elapsed since the last call.

Parameters:

  • req – Request (unused).

  • res – Response whose message field holds the elapsed seconds.

Returns:

True on success.

virtual ~RosAgent() = default#

Public Static Functions

static std::unique_ptr<RosAgent> create(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private, PluginFactory *factory, Configurable::ParamMap *param_map)#

Factory method to create a fully initialized ROS agent.

Avoids calling virtual methods in the constructor by separating construction from parameter setup.

Parameters:

  • nh – Public ROS node handle.

  • nh_private – Private ROS node handle for parameters.

  • factory – Plugin factory used to create the agent’s plugins.

  • param_map – Parameter map populated by the factory.

Returns:

A fully initialized RosAgent.

static void setupFactoryAndParams(PluginFactory &factory, Configurable::ParamMap *param_map, const ::ros::NodeHandle &nh_private)#

Static helper to initialize a PluginFactory and param map from a ROS namespace.

Parameters:

  • factory – Factory to read the parameter namespace through.

  • param_map – Output parameter map.

  • nh_private – Private node handle whose namespace is queried.

struct RosAgentMetrics : public open_apero::AgentMetrics#
#include <open_apero_ros/agent/ros_agent_metrics.h>

Extends AgentMetrics with ROS-specific timing fields.

Public Functions

inline virtual void declareFields(MetricsFields &fields) const override#

Declares this group’s fields as {column name, field pointer} pairs.

Parameters:

fields – Collector the fields are appended to in column order.

inline virtual void resetCycle() override#

Clears the per-cycle fields; the agent calls it once a cycle has selected its step, after the cycle’s CSV row (if any).

Public Members

double replan_time_ros_s = 0.0#

ROS-time elapsed since the previous replan call [s] (ros::Time::now() delta).

double ros_callback_s = 0.0#

Accumulated time spent inside ros::spinOnce() during the cycle.

double ros_time_s = 0.0#

ROS clock time at this cycle [s] (ros::Time::now()); row identity, not core-visible.

namespace marker_conversion_detail#

Functions

inline bool isPointListType(int type)#
namespace status_overlay_detail#

Functions

inline std::string escapeMarkup(const std::string &text)#

Variables

constexpr double kAscentEm = 0.928#
constexpr double kDescentEm = 0.236#
constexpr double kGlyphAdvanceEm = 0.602#
constexpr int kMarginPx = 8#
constexpr double kPixelsPerPoint = 96.0 / 72.0#
constexpr int kSlackPx = 2#
constexpr double kTextSizePt = 18.0#
inline void open_apero::installRosconsoleSink()#

Routes every open_apero logging::* message to rosconsole.

Installed once from main() after ros::init, before any agent is constructed, so both agent messages (Agent::printX forwards here) and free-function calls from code without an agent reach the same backend. rosconsole adds its own timestamp/severity/node-name formatting.

inline void open_apero::setLogLevelFromParam(const ::ros::NodeHandle &nh_private)#

Sets rosconsole verbosity AND the core min-severity from ~log_level.

Accepted values: debug, info, warn, error, fatal (default: info). Unknown values keep the default and emit a warning. Mapping both keeps the two filters consistent: sub-level messages are dropped before reaching rosconsole.

Parameters:

nh_private – Private node handle the parameter is read from.

ROS Plugins#

class PointsEvaluator : public open_apero::evaluator::SimulatedSensorEvaluator#
#include <open_apero_ros/plugin/evaluator/points_evaluator.h>

Evaluator that assigns gain to nodes based on proximity to discrete collectible point spheres scattered in the environment.

Points go from kUnseen to kObserved as the robot maps them, and from kUnseen or kObserved to kCollected as it passes within their radius. Gain is awarded only for kObserved points within collection radius.

Public Types

enum class PointStatus#

Lifecycle status of a point sphere.

Values:

enumerator kUnseen#
enumerator kObserved#
enumerator kCollected#

Public Functions

virtual double getRealAccumulatedGain() const override#

Return the total gain from all kCollected points.

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

Publish visualization markers and collectible point status for a given node.

Parameters:

  • markers – Output visualization markers.

  • node – The node to visualize.

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

Update point statuses (kObserved/kCollected) based on the traversed edge.

Parameters:

  • reached_node – The node the robot just navigated to.

  • traversed_edge – Edge used to reach the node, or nullptr.

explicit PointsEvaluator(AgentInterface &agent)#

Construct the evaluator.

Parameters:

agent – Reference to the parent agent interface.

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#

Read evaluator parameters and load point definitions from file.

Parameters:

param_map – Pointer to the parameter map.

struct PointSphere : public open_apero::GainAnchor#
#include <open_apero_ros/plugin/evaluator/points_evaluator.h>

Spherical collectible with gain and lifecycle state.

Public Functions

inline PointSphere(double gain, double x, double y, double z, double radius, PointStatus status, int id)#

Construct a PointSphere.

Parameters:

  • gain – Gain value awarded upon collection.

  • x – X position.

  • y – Y position.

  • z – Z position.

  • radius – Collection radius.

  • status – Initial status.

  • id – Unique identifier.

Public Members

int id#
Eigen::Vector3d position#
double radius#
PointStatus status = PointStatus::kUnseen#