Helper class that will publish results onto the ROS framework. More...

#include <ROS2Visualizer.h>

Public Member Functions
void	callback_inertial (const sensor_msgs::msg::Imu::SharedPtr msg)
	Callback for inertial information. More...

void	callback_monocular (const sensor_msgs::msg::Image::SharedPtr msg0, int cam_id0)
	Callback for monocular cameras information. More...

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. More...

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

void	setup_subscribers (std::shared_ptr< ov_core::YamlParser > parser)
	Will setup ROS subscribers and callbacks. More...

void	visualize ()
	Will visualize the system if we have new things. More...

void	visualize_final ()
	After the run has ended, print results. More...

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. More...

Protected Types
typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::msg::Image, sensor_msgs::msg::Image >	sync_pol

Protected Member Functions
void	publish_features ()
	Publish current features. More...

void	publish_groundtruth ()
	Publish groundtruth (if we have it) More...

void	publish_images ()
	Publish the active tracking image. More...

void	publish_loopclosure_information ()
	Publish loop-closure information of current pose and active track information. More...

void	publish_state ()
	Publish the current state. More...

Protected Attributes
std::shared_ptr< VioManager >	_app
	Core application of the filter system. More...

std::shared_ptr< rclcpp::Node >	_node
	Global node handler. More...

std::shared_ptr< Simulator >	_sim
	Simulator (is nullptr if we are not sim'ing) More...

std::map< int, double >	camera_last_timestamp

std::deque< ov_core::CameraData >	camera_queue

std::mutex	camera_queue_mtx

std::map< double, Eigen::Matrix< double, 17, 1 > >	gt_states

image_transport::Publisher	it_pub_loop_img_depth

image_transport::Publisher	it_pub_loop_img_depth_color

image_transport::Publisher	it_pub_tracks

double	last_visualization_timestamp = 0

double	last_visualization_timestamp_image = 0

std::shared_ptr< tf2_ros::TransformBroadcaster >	mTfBr

std::ofstream	of_state_est

std::ofstream	of_state_gt

std::ofstream	of_state_std

std::vector< geometry_msgs::msg::PoseStamped >	poses_gt

std::vector< geometry_msgs::msg::PoseStamped >	poses_imu

rclcpp::Publisher< nav_msgs::msg::Odometry >::SharedPtr	pub_loop_extrinsic

rclcpp::Publisher< sensor_msgs::msg::CameraInfo >::SharedPtr	pub_loop_intrinsics

rclcpp::Publisher< sensor_msgs::msg::PointCloud >::SharedPtr	pub_loop_point

rclcpp::Publisher< nav_msgs::msg::Odometry >::SharedPtr	pub_loop_pose

rclcpp::Publisher< nav_msgs::msg::Odometry >::SharedPtr	pub_odomimu

rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr	pub_pathgt

rclcpp::Publisher< nav_msgs::msg::Path >::SharedPtr	pub_pathimu

rclcpp::Publisher< sensor_msgs::msg::PointCloud2 >::SharedPtr	pub_points_aruco

rclcpp::Publisher< sensor_msgs::msg::PointCloud2 >::SharedPtr	pub_points_msckf

rclcpp::Publisher< sensor_msgs::msg::PointCloud2 >::SharedPtr	pub_points_sim

rclcpp::Publisher< sensor_msgs::msg::PointCloud2 >::SharedPtr	pub_points_slam

rclcpp::Publisher< geometry_msgs::msg::PoseStamped >::SharedPtr	pub_posegt

rclcpp::Publisher< geometry_msgs::msg::PoseWithCovarianceStamped >::SharedPtr	pub_poseimu

bool	publish_calibration_tf = true

bool	publish_global2imu_tf = true

boost::posix_time::ptime	rT1

boost::posix_time::ptime	rT2

bool	save_total_state = false

bool	start_time_set = false

rclcpp::Subscription< sensor_msgs::msg::Imu >::SharedPtr	sub_imu

std::vector< rclcpp::Subscription< sensor_msgs::msg::Image >::SharedPtr >	subs_cam

double	summed_mse_ori = 0.0

double	summed_mse_pos = 0.0

double	summed_nees_ori = 0.0

double	summed_nees_pos = 0.0

size_t	summed_number = 0

std::vector< std::shared_ptr< message_filters::Synchronizer< sync_pol > > >	sync_cam

std::vector< std::shared_ptr< message_filters::Subscriber< sensor_msgs::msg::Image > > >	sync_subs_cam

std::atomic< bool >	thread_update_running

Detailed Description

Helper class that will publish results onto the ROS framework.

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

Definition at line 78 of file ROS2Visualizer.h.

Member Typedef Documentation

◆ sync_pol

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image> ov_msckf::ROS2Visualizer::sync_pol

protected

Definition at line 161 of file ROS2Visualizer.h.

Constructor & Destructor Documentation

◆ ROS2Visualizer()

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

Definition at line 38 of file ROS2Visualizer.cpp.

Member Function Documentation

◆ callback_inertial()

void ROS2Visualizer::callback_inertial ( const sensor_msgs::msg::Imu::SharedPtr msg )

Callback for inertial information.

Definition at line 438 of file ROS2Visualizer.cpp.

◆ callback_monocular()

void ROS2Visualizer::callback_monocular	(	const sensor_msgs::msg::Image::SharedPtr	msg0,
		int	cam_id0
	)

Callback for monocular cameras information.

Definition at line 498 of file ROS2Visualizer.cpp.

◆ callback_stereo()

void ROS2Visualizer::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.

Definition at line 537 of file ROS2Visualizer.cpp.

◆ publish_features()

void ROS2Visualizer::publish_features ( )

protected

Publish current features.

Definition at line 674 of file ROS2Visualizer.cpp.

◆ publish_groundtruth()

void ROS2Visualizer::publish_groundtruth ( )

protected

Publish groundtruth (if we have it)

Definition at line 706 of file ROS2Visualizer.cpp.

◆ publish_images()

void ROS2Visualizer::publish_images ( )

protected

Publish the active tracking image.

Definition at line 646 of file ROS2Visualizer.cpp.

◆ publish_loopclosure_information()

void ROS2Visualizer::publish_loopclosure_information ( )

protected

Publish loop-closure information of current pose and active track information.

Definition at line 833 of file ROS2Visualizer.cpp.

◆ publish_state()

void ROS2Visualizer::publish_state ( )

protected

Publish the current state.

Definition at line 591 of file ROS2Visualizer.cpp.

◆ setup_subscribers()

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

Will setup ROS subscribers and callbacks.

Parameters

parser Configuration file parser

Definition at line 163 of file ROS2Visualizer.cpp.

◆ visualize()

void ROS2Visualizer::visualize ( )

Will visualize the system if we have new things.

Definition at line 221 of file ROS2Visualizer.cpp.

◆ visualize_final()

void ROS2Visualizer::visualize_final ( )

After the run has ended, print results.

Definition at line 352 of file ROS2Visualizer.cpp.

◆ visualize_odometry()

void ROS2Visualizer::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.

Definition at line 269 of file ROS2Visualizer.cpp.

Member Data Documentation

◆ _app

std::shared_ptr<VioManager> ov_msckf::ROS2Visualizer::_app

protected

Core application of the filter system.

Definition at line 142 of file ROS2Visualizer.h.

◆ _node

std::shared_ptr<rclcpp::Node> ov_msckf::ROS2Visualizer::_node

protected

Global node handler.

Definition at line 139 of file ROS2Visualizer.h.

◆ _sim

std::shared_ptr<Simulator> ov_msckf::ROS2Visualizer::_sim

protected

Simulator (is nullptr if we are not sim'ing)

Definition at line 145 of file ROS2Visualizer.h.

◆ camera_last_timestamp

std::map<int, double> ov_msckf::ROS2Visualizer::camera_last_timestamp

protected

Definition at line 192 of file ROS2Visualizer.h.

◆ camera_queue

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.

Definition at line 188 of file ROS2Visualizer.h.

◆ camera_queue_mtx

std::mutex ov_msckf::ROS2Visualizer::camera_queue_mtx

protected

Definition at line 189 of file ROS2Visualizer.h.

◆ gt_states

std::map<double, Eigen::Matrix<double, 17, 1> > ov_msckf::ROS2Visualizer::gt_states

protected

Definition at line 199 of file ROS2Visualizer.h.

◆ it_pub_loop_img_depth

image_transport::Publisher ov_msckf::ROS2Visualizer::it_pub_loop_img_depth

protected

Definition at line 148 of file ROS2Visualizer.h.

◆ it_pub_loop_img_depth_color

image_transport::Publisher ov_msckf::ROS2Visualizer::it_pub_loop_img_depth_color

protected

Definition at line 148 of file ROS2Visualizer.h.

◆ it_pub_tracks

image_transport::Publisher ov_msckf::ROS2Visualizer::it_pub_tracks

protected

Definition at line 148 of file ROS2Visualizer.h.

◆ last_visualization_timestamp

double ov_msckf::ROS2Visualizer::last_visualization_timestamp = 0

protected

Definition at line 195 of file ROS2Visualizer.h.

◆ last_visualization_timestamp_image

double ov_msckf::ROS2Visualizer::last_visualization_timestamp_image = 0

protected

Definition at line 196 of file ROS2Visualizer.h.

◆ mTfBr

std::shared_ptr<tf2_ros::TransformBroadcaster> ov_msckf::ROS2Visualizer::mTfBr

protected

Definition at line 156 of file ROS2Visualizer.h.

◆ of_state_est

std::ofstream ov_msckf::ROS2Visualizer::of_state_est

protected

Definition at line 208 of file ROS2Visualizer.h.

◆ of_state_gt

std::ofstream ov_msckf::ROS2Visualizer::of_state_gt

protected

Definition at line 208 of file ROS2Visualizer.h.

◆ of_state_std

std::ofstream ov_msckf::ROS2Visualizer::of_state_std

protected

Definition at line 208 of file ROS2Visualizer.h.

◆ poses_gt

std::vector<geometry_msgs::msg::PoseStamped> ov_msckf::ROS2Visualizer::poses_gt

protected

Definition at line 202 of file ROS2Visualizer.h.

◆ poses_imu

std::vector<geometry_msgs::msg::PoseStamped> ov_msckf::ROS2Visualizer::poses_imu

protected

Definition at line 166 of file ROS2Visualizer.h.

◆ pub_loop_extrinsic

rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr ov_msckf::ROS2Visualizer::pub_loop_extrinsic

protected

Definition at line 153 of file ROS2Visualizer.h.

◆ pub_loop_intrinsics

rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr ov_msckf::ROS2Visualizer::pub_loop_intrinsics

protected

Definition at line 155 of file ROS2Visualizer.h.

◆ pub_loop_point

rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr ov_msckf::ROS2Visualizer::pub_loop_point

protected

Definition at line 154 of file ROS2Visualizer.h.

◆ pub_loop_pose

rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr ov_msckf::ROS2Visualizer::pub_loop_pose

protected

Definition at line 153 of file ROS2Visualizer.h.

◆ pub_odomimu

rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr ov_msckf::ROS2Visualizer::pub_odomimu

protected

Definition at line 150 of file ROS2Visualizer.h.

◆ pub_pathgt

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr ov_msckf::ROS2Visualizer::pub_pathgt

protected

Definition at line 169 of file ROS2Visualizer.h.

◆ pub_pathimu

rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr ov_msckf::ROS2Visualizer::pub_pathimu

protected

Definition at line 151 of file ROS2Visualizer.h.

◆ pub_points_aruco

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ov_msckf::ROS2Visualizer::pub_points_aruco

protected

Definition at line 152 of file ROS2Visualizer.h.

◆ pub_points_msckf

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ov_msckf::ROS2Visualizer::pub_points_msckf

protected

Definition at line 152 of file ROS2Visualizer.h.

◆ pub_points_sim

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ov_msckf::ROS2Visualizer::pub_points_sim

protected

Definition at line 152 of file ROS2Visualizer.h.

◆ pub_points_slam

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr ov_msckf::ROS2Visualizer::pub_points_slam

protected

Definition at line 152 of file ROS2Visualizer.h.

◆ pub_posegt

rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr ov_msckf::ROS2Visualizer::pub_posegt

protected

Definition at line 170 of file ROS2Visualizer.h.

◆ pub_poseimu

rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr ov_msckf::ROS2Visualizer::pub_poseimu

protected

Definition at line 149 of file ROS2Visualizer.h.

◆ publish_calibration_tf

bool ov_msckf::ROS2Visualizer::publish_calibration_tf = true

protected

Definition at line 204 of file ROS2Visualizer.h.

◆ publish_global2imu_tf

bool ov_msckf::ROS2Visualizer::publish_global2imu_tf = true

protected

Definition at line 203 of file ROS2Visualizer.h.

◆ rT1

boost::posix_time::ptime ov_msckf::ROS2Visualizer::rT1

protected

Definition at line 179 of file ROS2Visualizer.h.

◆ rT2

boost::posix_time::ptime ov_msckf::ROS2Visualizer::rT2

protected

Definition at line 179 of file ROS2Visualizer.h.

◆ save_total_state

bool ov_msckf::ROS2Visualizer::save_total_state = false

protected

Definition at line 207 of file ROS2Visualizer.h.

◆ start_time_set

bool ov_msckf::ROS2Visualizer::start_time_set = false

protected

Definition at line 178 of file ROS2Visualizer.h.

◆ sub_imu

rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr ov_msckf::ROS2Visualizer::sub_imu

protected

Definition at line 159 of file ROS2Visualizer.h.

◆ subs_cam

std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> ov_msckf::ROS2Visualizer::subs_cam

protected

Definition at line 160 of file ROS2Visualizer.h.

◆ summed_mse_ori

double ov_msckf::ROS2Visualizer::summed_mse_ori = 0.0

protected

Definition at line 171 of file ROS2Visualizer.h.

◆ summed_mse_pos

double ov_msckf::ROS2Visualizer::summed_mse_pos = 0.0

protected

Definition at line 172 of file ROS2Visualizer.h.

◆ summed_nees_ori

double ov_msckf::ROS2Visualizer::summed_nees_ori = 0.0

protected

Definition at line 173 of file ROS2Visualizer.h.

◆ summed_nees_pos

double ov_msckf::ROS2Visualizer::summed_nees_pos = 0.0

protected

Definition at line 174 of file ROS2Visualizer.h.

◆ summed_number

size_t ov_msckf::ROS2Visualizer::summed_number = 0

protected

Definition at line 175 of file ROS2Visualizer.h.

◆ sync_cam

std::vector<std::shared_ptr<message_filters::Synchronizer<sync_pol> > > ov_msckf::ROS2Visualizer::sync_cam

protected

Definition at line 162 of file ROS2Visualizer.h.

◆ sync_subs_cam

std::vector<std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image> > > ov_msckf::ROS2Visualizer::sync_subs_cam

protected

Definition at line 163 of file ROS2Visualizer.h.

◆ thread_update_running

std::atomic<bool> ov_msckf::ROS2Visualizer::thread_update_running

protected

Definition at line 182 of file ROS2Visualizer.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ sync_pol

Constructor & Destructor Documentation

◆ ROS2Visualizer()

Member Function Documentation

◆ callback_inertial()

◆ callback_monocular()

◆ callback_stereo()

◆ publish_features()

◆ publish_groundtruth()

◆ publish_images()

◆ publish_loopclosure_information()

◆ publish_state()

◆ setup_subscribers()

◆ visualize()

◆ visualize_final()

◆ visualize_odometry()

Member Data Documentation

◆ _app

◆ _node

◆ _sim

◆ camera_last_timestamp

◆ camera_queue

◆ camera_queue_mtx

◆ gt_states

◆ it_pub_loop_img_depth

◆ it_pub_loop_img_depth_color

◆ it_pub_tracks

◆ last_visualization_timestamp

◆ last_visualization_timestamp_image

◆ mTfBr

◆ of_state_est

◆ of_state_gt

◆ of_state_std

◆ poses_gt

◆ poses_imu

◆ pub_loop_extrinsic

◆ pub_loop_intrinsics

◆ pub_loop_point

◆ pub_loop_pose

◆ pub_odomimu

◆ pub_pathgt

◆ pub_pathimu

◆ pub_points_aruco

◆ pub_points_msckf

◆ pub_points_sim

◆ pub_points_slam

◆ pub_posegt

◆ pub_poseimu

◆ publish_calibration_tf

◆ publish_global2imu_tf

◆ rT1

◆ rT2

◆ save_total_state

◆ start_time_set

◆ sub_imu

◆ subs_cam

◆ summed_mse_ori

◆ summed_mse_pos

◆ summed_nees_ori

◆ summed_nees_pos

◆ summed_number

◆ sync_cam

◆ sync_subs_cam

◆ thread_update_running