ov_msckf::ROS2Visualizer class

Helper class that will publish results onto the ROS framework.

Reference

Also save to file the current total state and covariance along with the groundtruth if we are simulating. We visualize the following things:

State of the system on TF, pose message, and path
Image of our tracker
Our different features (SLAM, MSCKF, ARUCO)
Groundtruth trajectory if we have it

Constructors, destructors, conversion operators

ROS2Visualizer(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<VioManager> app, std::shared_ptr<Simulator> sim = nullptr): Default constructor.

Public functions

void setup_subscribers(std::shared_ptr<ov_core::YamlParser> parser): Will setup ROS subscribers and callbacks.
void visualize(): Will visualize the system if we have new things.
void visualize_odometry(double timestamp): Will publish our odometry message for the current timestep. This will take the current state estimate and get the propagated pose to the desired time. This can be used to get pose estimates on systems which require high frequency pose estimates.
void visualize_final(): After the run has ended, print results.
void callback_inertial(const sensor_msgs::msg::Imu::SharedPtr msg): Callback for inertial information.
void callback_monocular(const sensor_msgs::msg::Image::SharedPtr msg0, int cam_id0): Callback for monocular cameras information.
void callback_stereo(const sensor_msgs::msg::Image::ConstSharedPtr msg0, const sensor_msgs::msg::Image::ConstSharedPtr msg1, int cam_id0, int cam_id1): Callback for synchronized stereo camera information.

Protected functions

void publish_state(): Publish the current state.
void publish_images(): Publish the active tracking image.
void publish_features(): Publish current features.
void publish_groundtruth(): Publish groundtruth (if we have it)
void publish_loopclosure_information(): Publish loop-closure information of current pose and active track information.

Protected variables

std::shared_ptr<rclcpp::Node> _node: Global node handler.
std::shared_ptr<VioManager> _app: Core application of the filter system.
std::shared_ptr<Simulator> _sim: Simulator (is nullptr if we are not sim'ing)
std::deque<ov_core::CameraData> camera_queue

Function documentation

ov_msckf::ROS2Visualizer::ROS2Visualizer(std::shared_ptr<rclcpp::Node> node, std::shared_ptr<VioManager> app, std::shared_ptr<Simulator> sim = nullptr)

Default constructor.

Parameters
node	ROS node pointer
app	Core estimator manager
sim	Simulator if we are simulating

void ov_msckf::ROS2Visualizer::setup_subscribers(std::shared_ptr<ov_core::YamlParser> parser)

Will setup ROS subscribers and callbacks.

Parameters
parser	Configuration file parser

Variable documentation

std::deque<ov_core::CameraData> ov_msckf::ROS2Visualizer::camera_queue protected

Queue up camera measurements sorted by time and trigger once we have exactly one IMU measurement with timestamp newer than the camera measurement This also handles out-of-order camera measurements, which is rare, but a nice feature to have for general robustness to bad camera drivers.