Program Listing for File data-publisher-flow.h¶
↰ Return to documentation for file (applications/maplab-node/include/maplab-node/data-publisher-flow.h
)
#ifndef MAPLAB_NODE_DATA_PUBLISHER_FLOW_H_
#define MAPLAB_NODE_DATA_PUBLISHER_FLOW_H_
#include <memory>
#include <string>
#include <maplab-common/conversions.h>
#include <maplab-common/timeout-counter.h>
#include <message-flow/message-flow.h>
#include <tf/transform_broadcaster.h>
#include <visualization/common-rviz-visualization.h>
#include <visualization/rviz-visualization-sink.h>
#include <visualization/viwls-graph-plotter.h>
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <ros/ros.h>
#pragma GCC diagnostic pop
#include "maplab-node/flow-topics.h"
namespace maplab {
class DataPublisherFlow {
public:
const std::string kTopicPoseMission = "T_M_B";
const std::string kTopicPoseGlobal = "T_G_I";
const std::string kTopicBaseframe = "T_G_M";
const std::string kTopicVelocity = "velocity_I";
const std::string kTopicBiasAcc = "bias_acc";
const std::string kTopicBiasGyro = "bias_gyro";
const std::string kCameraExtrinsicTopic = "cam_T_C_B";
const std::string kTopicLocalizationResult = "localization_T_G_B";
const std::string kTopicLocalizationResultFused = "localization_fused_T_G_B";
explicit DataPublisherFlow(const vi_map::SensorManager& sensor_manager);
void attachToMessageFlow(message_flow::MessageFlow* flow);
void visualizeMap(const vi_map::VIMap& vi_map) const;
private:
void registerPublishers();
void publishOdometryState(
int64_t timestamp_ns, const vio::ViNodeState& vinode);
void publishVinsState(
int64_t timestamp_ns, const vio::ViNodeState& vinode,
const bool has_T_G_M, const aslam::Transformation& T_G_M);
void stateDebugCallback(
const vio::ViNodeState& vinode, const bool has_T_G_M,
const aslam::Transformation& T_G_M);
void localizationCallback(
const common::LocalizationResult& localization_result,
const bool is_fused_localization_result);
const vi_map::SensorManager& sensor_manager_;
std::unique_ptr<visualization::ViwlsGraphRvizPlotter> plotter_;
ros::NodeHandle node_handle_;
ros::Publisher pub_pose_T_M_B_;
ros::Publisher pub_pose_T_G_I_;
ros::Publisher pub_baseframe_T_G_M_;
ros::Publisher pub_velocity_I_;
ros::Publisher pub_imu_acc_bias_;
ros::Publisher pub_imu_gyro_bias_;
ros::Publisher pub_extrinsics_T_C_Bs_;
ros::Publisher pub_localization_result_T_G_B_;
ros::Publisher pub_fused_localization_result_T_G_B_;
common::TimeoutCounter map_publisher_timeout_;
visualization::SphereVector T_M_B_spheres_;
visualization::SphereVector T_G_I_spheres_;
visualization::SphereVector T_G_B_raw_loc_spheres_;
visualization::SphereVector T_G_B_fused_loc_spheres_;
aslam::Transformation latest_T_G_M_;
};
} // namespace maplab
#endif // MAPLAB_NODE_DATA_PUBLISHER_FLOW_H_