From d6260e0ffec830b3e7c1f5599222ac0d0328cc15 Mon Sep 17 00:00:00 2001 From: Dmitry Klimenkov Date: Fri, 19 Aug 2022 10:31:13 +0200 Subject: [PATCH 1/2] Add blacklisted topics --- .../include/rosbag2_snapshot/snapshotter.hpp | 2 + rosbag2_snapshot/src/snapshotter.cpp | 49 ++++++++++++++++--- 2 files changed, 44 insertions(+), 7 deletions(-) diff --git a/rosbag2_snapshot/include/rosbag2_snapshot/snapshotter.hpp b/rosbag2_snapshot/include/rosbag2_snapshot/snapshotter.hpp index 1a587be..4b2bf80 100644 --- a/rosbag2_snapshot/include/rosbag2_snapshot/snapshotter.hpp +++ b/rosbag2_snapshot/include/rosbag2_snapshot/snapshotter.hpp @@ -131,6 +131,8 @@ struct SnapshotterOptions // Provides list of topics to snapshot and their limit configurations topics_t topics_; + topics_t blacklist_topics_; + SnapshotterOptions( rclcpp::Duration default_duration_limit = rclcpp::Duration(30s), int32_t default_memory_limit = -1); diff --git a/rosbag2_snapshot/src/snapshotter.cpp b/rosbag2_snapshot/src/snapshotter.cpp index 8d0681f..ba57cce 100644 --- a/rosbag2_snapshot/src/snapshotter.cpp +++ b/rosbag2_snapshot/src/snapshotter.cpp @@ -85,6 +85,8 @@ bool SnapshotterOptions::addTopic( rclcpp::Duration duration, int32_t memory) { + if(blacklist_topics_.find(topic_details) != blacklist_topics_.end()) return false; + SnapshotterTopicOptions ops(duration, memory); std::pair ret; ret = topics_.emplace(topic_details, ops); @@ -287,6 +289,7 @@ Snapshotter::~Snapshotter() void Snapshotter::parseOptionsFromParams() { std::vector topics{}; + std::vector blacklist_topics{}; try { options_.default_duration_limit_ = rclcpp::Duration::from_seconds( @@ -319,10 +322,37 @@ void Snapshotter::parseOptionsFromParams() } } - if (topics.size() > 0) { - options_.all_topics_ = false; + try { + blacklist_topics = declare_parameter>( + "blacklist_topics", std::vector{}); + } catch (const rclcpp::ParameterTypeException & ex) { + if (std::string{ex.what()}.find("not set") == std::string::npos) { + RCLCPP_ERROR(get_logger(), "blacklist_topics must be an array of strings."); + throw ex; + } + } - for (const auto & topic : topics) { + try { + if(topics.size() > 0 && blacklist_topics.size() > 0) + throw rclcpp::exceptions::InvalidParametersException("Non-consisting arguments."); + + } catch (const rclcpp::exceptions::InvalidParametersException& ex){ + RCLCPP_ERROR(get_logger(), "Use either `topics` param (record topics which are passed) " + "or `blacklist_topics` (record everything except the ones)"); + throw ex; + } + + if (topics.size() > 0 || blacklist_topics.size() > 0) { + std::vector topics_to_parse; + if(topics.size() > 0){ + topics_to_parse = topics; + options_.all_topics_ = false; + } else { + topics_to_parse = blacklist_topics; + options_.all_topics_ = true; + } + + for (const auto & topic : topics_to_parse) { std::string prefix = "topic_details." + topic; std::string topic_type{}; SnapshotterTopicOptions opts{}; @@ -364,9 +394,12 @@ void Snapshotter::parseOptionsFromParams() TopicDetails dets{}; dets.name = topic; dets.type = topic_type; - - options_.topics_.insert( - SnapshotterOptions::topics_t::value_type(dets, opts)); + + if(topics.size() > 0){ + options_.topics_.insert(SnapshotterOptions::topics_t::value_type(dets, opts)); + } else { + options_.blacklist_topics_.insert(SnapshotterOptions::topics_t::value_type(dets, opts)); + } } } else { options_.all_topics_ = true; @@ -433,10 +466,12 @@ void Snapshotter::subscribe( opts.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; opts.topic_stats_options.publish_topic = topic_details.name + "/statistics"; + bool is_image = topic_details.type == "sensor_msgs/msg/Image"; + auto sub = create_generic_subscription( topic_details.name, topic_details.type, - rclcpp::QoS{10}, + is_image ? rclcpp::SensorDataQoS() : rclcpp::QoS{10}, std::bind(&Snapshotter::topicCb, this, _1, queue), opts ); From d28926b3532bdefd3e9a20e57f0cb9260479e8a3 Mon Sep 17 00:00:00 2001 From: Dmitry Klimenkov Date: Wed, 31 Aug 2022 13:04:18 +0200 Subject: [PATCH 2/2] Add is_blacklist argument --- rosbag2_snapshot/src/snapshotter.cpp | 43 ++++++++-------------------- 1 file changed, 12 insertions(+), 31 deletions(-) diff --git a/rosbag2_snapshot/src/snapshotter.cpp b/rosbag2_snapshot/src/snapshotter.cpp index ba57cce..cb6d069 100644 --- a/rosbag2_snapshot/src/snapshotter.cpp +++ b/rosbag2_snapshot/src/snapshotter.cpp @@ -289,7 +289,7 @@ Snapshotter::~Snapshotter() void Snapshotter::parseOptionsFromParams() { std::vector topics{}; - std::vector blacklist_topics{}; + bool is_blacklist = true; try { options_.default_duration_limit_ = rclcpp::Duration::from_seconds( @@ -323,36 +323,19 @@ void Snapshotter::parseOptionsFromParams() } try { - blacklist_topics = declare_parameter>( - "blacklist_topics", std::vector{}); + is_blacklist = declare_parameter( + "is_blacklist", true); } catch (const rclcpp::ParameterTypeException & ex) { if (std::string{ex.what()}.find("not set") == std::string::npos) { - RCLCPP_ERROR(get_logger(), "blacklist_topics must be an array of strings."); + RCLCPP_ERROR(get_logger(), "is_blacklist must be a bool."); throw ex; } } - try { - if(topics.size() > 0 && blacklist_topics.size() > 0) - throw rclcpp::exceptions::InvalidParametersException("Non-consisting arguments."); - - } catch (const rclcpp::exceptions::InvalidParametersException& ex){ - RCLCPP_ERROR(get_logger(), "Use either `topics` param (record topics which are passed) " - "or `blacklist_topics` (record everything except the ones)"); - throw ex; - } + if (topics.size() > 0) { + options_.all_topics_ = is_blacklist ? true : false; - if (topics.size() > 0 || blacklist_topics.size() > 0) { - std::vector topics_to_parse; - if(topics.size() > 0){ - topics_to_parse = topics; - options_.all_topics_ = false; - } else { - topics_to_parse = blacklist_topics; - options_.all_topics_ = true; - } - - for (const auto & topic : topics_to_parse) { + for (const auto & topic : topics) { std::string prefix = "topic_details." + topic; std::string topic_type{}; SnapshotterTopicOptions opts{}; @@ -395,11 +378,9 @@ void Snapshotter::parseOptionsFromParams() dets.name = topic; dets.type = topic_type; - if(topics.size() > 0){ - options_.topics_.insert(SnapshotterOptions::topics_t::value_type(dets, opts)); - } else { - options_.blacklist_topics_.insert(SnapshotterOptions::topics_t::value_type(dets, opts)); - } + if(is_blacklist) options_.blacklist_topics_.insert(SnapshotterOptions::topics_t::value_type(dets, opts)); + else options_.topics_.insert(SnapshotterOptions::topics_t::value_type(dets, opts)); + } } else { options_.all_topics_ = true; @@ -678,8 +659,8 @@ void Snapshotter::pollTopics() } if (name_type.second.size() > 1) { - RCLCPP_ERROR(get_logger(), "Subscribed topic has more than one associated type."); - return; + RCLCPP_INFO_STREAM_ONCE(get_logger(), "Subscribed topic has more than one associated type " << name_type.first ); + continue; } TopicDetails details{};