Support specifying patterns for matching topics at run-time

rosbag_snapshot/CMakeLists.txt

@@ -1,12 +1,18 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(rosbag_snapshot)
 
-find_package(catkin REQUIRED COMPONENTS rosbag rosbag_snapshot_msgs roscpp rosgraph_msgs std_srvs topic_tools)
+set(PACKAGE_DEPENDENCIES rosbag rosbag_snapshot_msgs roscpp rosgraph_msgs std_srvs topic_tools)
+set(PACKAGE_CATKIN_COMPONENTS ${PACKAGE_DEPENDENCIES})
+if(CATKIN_ENABLE_TESTING)
+  list(APPEND PACKAGE_CATKIN_COMPONENTS roslint rostest)
+endif()
+
+find_package(catkin REQUIRED COMPONENTS ${PACKAGE_CATKIN_COMPONENTS})
 
-find_package(Boost REQUIRED COMPONENTS program_options)
+find_package(Boost REQUIRED COMPONENTS program_options regex)
 
 catkin_package(
-    CATKIN_DEPENDS rosbag rosbag_snapshot_msgs roscpp rosgraph_msgs std_srvs topic_tools
+    CATKIN_DEPENDS ${PACKAGE_DEPENDENCIES}
     INCLUDE_DIRS include
     LIBRARIES rosbag_snapshot
 )
@@ -21,7 +27,6 @@ add_dependencies(snapshot ${catkin_EXPORTED_TARGETS})
 target_link_libraries(snapshot rosbag_snapshot ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin REQUIRED COMPONENTS roslint rostest)
   roslint_python(test)
   roslint_cpp()
   roslint_add_test()

rosbag_snapshot/include/rosbag_snapshot/snapshotter.h

@@ -37,6 +37,7 @@
 #include <boost/atomic.hpp>
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/shared_mutex.hpp>
+#include <boost/regex.hpp>
 #include <ros/ros.h>
 #include <ros/time.h>
 #include <rosbag_snapshot_msgs/TriggerSnapshot.h>
@@ -51,6 +52,8 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <memory>
+#include <unordered_map>
 
 namespace rosbag_snapshot
 {
@@ -86,6 +89,27 @@ struct ROSBAG_DECL SnapshotterTopicOptions
                           int32_t memory_limit = INHERIT_MEMORY_LIMIT, int32_t count_limit = INHERIT_COUNT_LIMIT);
 };
 
+/* Regular expression matching fully qualified topic names that should be tracked.
+ */
+class ROSBAG_DECL SnapshotterTopicPattern
+{
+  public:
+  SnapshotterTopicPattern(const std::string &pattern, const SnapshotterTopicOptions &topic_options);
+
+  const boost::regex& get_pattern() const;
+  const SnapshotterTopicOptions& get_topic_options() const;
+
+  // Return true if pattern matches the given topic name exactly.
+  bool matches(const std::string &topic) const;
+
+  private:
+  // Maximum difference in time from newest and oldest message in buffer before older messages are removed
+  boost::regex pattern_;
+  SnapshotterTopicOptions topic_options_;
+};
+typedef std::shared_ptr<SnapshotterTopicPattern> SnapshotterTopicPatternPtr;
+typedef std::shared_ptr<const SnapshotterTopicPattern> SnapshotterTopicPatternConstPtr;
+
 /* Configuration for the Snapshotter node. Contains default limits for memory and duration
  * and a map of topics to their limits which may override the defaults.
  */
@@ -110,6 +134,12 @@ struct ROSBAG_DECL SnapshotterOptions
   // Provides list of topics to snapshot and their limit configurations
   topics_t topics_;
 
+  typedef std::vector<SnapshotterTopicPatternConstPtr> patterns_t;
+  // List of regexs that match topics which should be tracked by the recorder.
+  patterns_t patterns_;
+  // Cache of the first pattern that has matched a given topic (or nullptr if no pattern matched).
+  std::unordered_map<std::string, SnapshotterTopicPatternConstPtr> matching_patterns_cache_;
+
   SnapshotterOptions(ros::Duration default_duration_limit = ros::Duration(30), int32_t default_memory_limit = -1,
                      int32_t default_count_limit = -1, ros::Duration status_period = ros::Duration(1),
                      bool clear_buffer = true);
@@ -119,6 +149,20 @@ struct ROSBAG_DECL SnapshotterOptions
                 ros::Duration duration_limit = SnapshotterTopicOptions::INHERIT_DURATION_LIMIT,
                 int32_t memory_limit = SnapshotterTopicOptions::INHERIT_MEMORY_LIMIT,
                 int32_t count_limit = SnapshotterTopicOptions::INHERIT_COUNT_LIMIT);
+  // Add a new topic pattern to the configuration, returns false if the pattern is invalid
+  bool addPattern(std::string const& pattern,
+                  ros::Duration duration_limit = SnapshotterTopicOptions::INHERIT_DURATION_LIMIT,
+                  int32_t memory_limit = SnapshotterTopicOptions::INHERIT_MEMORY_LIMIT,
+                  int32_t count_limit = SnapshotterTopicOptions::INHERIT_COUNT_LIMIT);
+
+  // Add a new topic or topic pattern to the configuration, returns false if the topic was already present
+  bool addTopicOrPattern(std::string const& topic_or_pattern,
+                         ros::Duration duration_limit = SnapshotterTopicOptions::INHERIT_DURATION_LIMIT,
+                         int32_t memory_limit = SnapshotterTopicOptions::INHERIT_MEMORY_LIMIT,
+                         int32_t count_limit = SnapshotterTopicOptions::INHERIT_COUNT_LIMIT);
+
+  // Return the first matching topic pattern that matches the given topic, otherwise null.
+  SnapshotterTopicPatternConstPtr findFirstMatchingPattern(const std::string &topic);
 };
 
 /* Stores a buffered message of an ambiguous type and it's associated metadata (time of arrival, connection data),

rosbag_snapshot/src/snapshot.cpp

@@ -116,7 +116,7 @@ bool parseVariablesMap(SnapshotterOptions& opts, po::variables_map const& vm)
     std::vector<std::string> topics = vm["topic"].as<std::vector<std::string> >();
     for (const std::string& str : topics)
     {
-      opts.addTopic(str);
+      opts.addTopicOrPattern(str);
     }
   }
   opts.default_memory_limit_ = static_cast<int>(MB_TO_BYTES * vm["size"].as<double>());
@@ -191,7 +191,7 @@ void appendParamOptions(ros::NodeHandle& nh, SnapshotterOptions& opts)
     // If it is just a string, add this topic
     if (topic_value.getType() == XmlRpcValue::TypeString)
     {
-      opts.addTopic(topic_value);
+      opts.addTopicOrPattern(topic_value);
     }
     else if (topic_value.getType() == XmlRpcValue::TypeStruct)
     {
@@ -249,7 +249,7 @@ void appendParamOptions(ros::NodeHandle& nh, SnapshotterOptions& opts)
         else
           ROS_FATAL("err");
       }
-      opts.addTopic(topic, dur, mem, cnt);
+      opts.addTopicOrPattern(topic, dur, mem, cnt);
     }
     else
       ROS_ASSERT_MSG(false, "Parameter invalid for topic %lu", i);
@@ -284,7 +284,7 @@ int main(int argc, char** argv)
   appendParamOptions(private_nh, opts);
 
   // Exit if not topics selected
-  if (!opts.topics_.size() && !opts.all_topics_)
+  if (!opts.topics_.size() && !opts.patterns_.size() && !opts.all_topics_)
   {
     ROS_FATAL("No topics selected. Exiting.");
     return 1;

rosbag_snapshot/src/snapshotter.cpp

@@ -62,6 +62,11 @@ const ros::Duration SnapshotterTopicOptions::INHERIT_DURATION_LIMIT = ros::Durat
 const int32_t SnapshotterTopicOptions::INHERIT_MEMORY_LIMIT = 0;
 const int32_t SnapshotterTopicOptions::INHERIT_COUNT_LIMIT = 0;
 
+static bool is_topic_name_pattern(const std::string& s)
+{
+  return s.find_first_of(".*+?()[]{}\\") != std::string::npos;
+}
+
 SnapshotterTopicOptions::SnapshotterTopicOptions(ros::Duration duration_limit, int32_t memory_limit,
                                                  int32_t count_limit)
   : duration_limit_(duration_limit), memory_limit_(memory_limit), count_limit_(count_limit)
@@ -76,6 +81,8 @@ SnapshotterOptions::SnapshotterOptions(ros::Duration default_duration_limit, int
   , status_period_(status_period)
   , clear_buffer_(clear_buffer)
   , topics_()
+  , patterns_()
+  , matching_patterns_cache_()
 {
 }
 
@@ -87,6 +94,71 @@ bool SnapshotterOptions::addTopic(std::string const& topic, ros::Duration durati
   return ret.second;
 }
 
+bool SnapshotterOptions::addPattern(std::string const& pattern, ros::Duration duration, int32_t memory, int32_t count)
+{
+  SnapshotterTopicOptions ops(duration, memory, count);
+  try
+  {
+    patterns_.emplace_back(std::make_shared<SnapshotterTopicPattern>(pattern, ops));
+    ROS_INFO("Added topic pattern: %s", pattern.c_str());
+    return true;
+  }
+  catch (boost::regex_error& e)
+  {
+    ROS_ERROR("Invalid topic pattern '%s': %s", pattern.c_str(), e.what());
+    return false;
+  }
+}
+
+bool SnapshotterOptions::addTopicOrPattern(std::string const& topic_or_pattern,
+                                           ros::Duration duration, int32_t memory, int32_t count)
+{
+  if (is_topic_name_pattern(topic_or_pattern))
+    return addPattern(topic_or_pattern, duration, memory, count);
+  else
+    return addTopic(topic_or_pattern, duration, memory, count);
+}
+
+SnapshotterTopicPatternConstPtr SnapshotterOptions::findFirstMatchingPattern(const std::string &topic)
+{
+  if (patterns_.empty())
+    return nullptr;
+  auto it = matching_patterns_cache_.find(topic);
+  if (it != matching_patterns_cache_.end())
+    return it->second;
+  SnapshotterTopicPatternConstPtr matching_pattern = nullptr;
+  for (auto& pattern : patterns_)
+    if (pattern->matches(topic))
+    {
+      matching_pattern = pattern;
+      break;
+    }
+  matching_patterns_cache_[topic] = matching_pattern;
+  return matching_pattern;
+}
+
+SnapshotterTopicPattern::SnapshotterTopicPattern(const std::string &pattern,
+                                                 const SnapshotterTopicOptions &topic_options)
+  : pattern_(pattern)
+  , topic_options_(topic_options)
+{
+}
+
+const boost::regex& SnapshotterTopicPattern::get_pattern() const
+{
+  return pattern_;
+}
+
+const SnapshotterTopicOptions& SnapshotterTopicPattern::get_topic_options() const
+{
+  return topic_options_;
+}
+
+bool SnapshotterTopicPattern::matches(const std::string &topic) const
+{
+  return boost::regex_match(topic, pattern_);
+}
+
 SnapshotterClientOptions::SnapshotterClientOptions() : action_(SnapshotterClientOptions::TRIGGER_WRITE)
 {
 }
@@ -562,6 +634,14 @@ void Snapshotter::pollTopics(ros::TimerEvent const& e, rosbag_snapshot::Snapshot
     for (ros::master::TopicInfo const& t : topics)
     {
       std::string topic = t.name;
+      SnapshotterTopicOptions topic_options;
+      if (!options->all_topics_)
+      {
+        SnapshotterTopicPatternConstPtr matching_topic_pattern = options->findFirstMatchingPattern(topic);
+        if (matching_topic_pattern == nullptr)
+          continue;
+        topic_options = matching_topic_pattern->get_topic_options();
+      }
       if (options->addTopic(topic))
       {
         SnapshotterTopicOptions topic_options;
@@ -606,10 +686,9 @@ int Snapshotter::run()
     status_timer_ = nh_.createTimer(options_.status_period_, &Snapshotter::publishStatus, this);
 
   // Start timer to poll ROS master for topics
-  if (options_.all_topics_)
-    poll_topic_timer_ = nh_.createTimer(ros::Duration(1.0),
-                                        boost::bind(&Snapshotter::pollTopics, this,
-                                                    boost::placeholders::_1, &options_));
+  poll_topic_timer_ = nh_.createTimer(ros::Duration(1.0),
+                                      boost::bind(&Snapshotter::pollTopics, this,
+                                                  boost::placeholders::_1, &options_));
 
   // Use multiple callback threads
   ros::MultiThreadedSpinner spinner(4);  // Use 4 threads