diff --git a/include/visualizer.h b/include/visualizer.h index 988da90a5400cd9e06b443a8b5aaf012cae22584..21b89f94d9082a91d27dded1c75a812a9554938e 100644 --- a/include/visualizer.h +++ b/include/visualizer.h @@ -66,7 +66,7 @@ class Visualizer // Options std::string map_frame_id_; - bool viz_factors_, viz_landmarks_, viz_trajectory_, viz_overlapped_factors_; + bool viz_overlapped_factors_; double viz_scale_, factors_width_, factors_absolute_height_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_; std_msgs::ColorRGBA frame_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_; diff --git a/src/visualizer.cpp b/src/visualizer.cpp index 79d5739c99751bd99832104740a7f078b2a8d40b..af2aad32b34f231ff5f5f3824e7ff5d0883684a8 100644 --- a/src/visualizer.cpp +++ b/src/visualizer.cpp @@ -12,10 +12,7 @@ Visualizer::Visualizer() : void Visualizer::initialize(ros::NodeHandle& nh) { // Load options --------------------------------------------------- - nh.param<bool>( "viz_factors", viz_factors_, true); nh.param<bool>( "viz_overlapped_factors", viz_overlapped_factors_, false); - nh.param<bool>( "viz_landmarks", viz_landmarks_, true); - nh.param<bool>( "viz_trajectory", viz_trajectory_, true); // viz parameters nh.param<std::string>( "map_frame_id", map_frame_id_, "map"); nh.param<double>( "viz_scale", viz_scale_, 1); @@ -58,12 +55,9 @@ void Visualizer::initialize(ros::NodeHandle& nh) nh.param<float>( "factor_geom_color_a", factor_geom_color_.a, 1); // init publishers --------------------------------------------------- - if (viz_factors_) - factors_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("factors", 1); - if (viz_landmarks_) - landmarks_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("landmarks", 1); - if (viz_trajectory_) - trajectory_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("trajectory", 1); + factors_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("factors", 1); + landmarks_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("landmarks", 1); + trajectory_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("trajectory", 1); // init markers --------------------------------------------------- // factor markers message @@ -122,11 +116,11 @@ void Visualizer::initialize(ros::NodeHandle& nh) void Visualizer::visualize(const ProblemPtr problem) { - if (viz_factors_) + if (factors_publisher_.getNumSubscribers() != 0) publishFactors(problem); - if (viz_landmarks_) + if (landmarks_publisher_.getNumSubscribers() != 0) publishLandmarks(problem); - if (viz_trajectory_) + if (trajectory_publisher_.getNumSubscribers() != 0) publishTrajectory(problem); }