Feature/new tf utils

ianvs/CMakeLists.txt

@@ -36,13 +36,14 @@ find_package(pybind11 CONFIG REQUIRED)
 add_library(ament_shim INTERFACE)
 ament_target_dependencies(ament_shim INTERFACE tf2_msgs tf2_ros)
 
-#######################################################################################
+# ######################################################################################
 # Libraries
-#######################################################################################
+# ######################################################################################
 
 add_library(
   ${PROJECT_NAME}
   src/detail/string_transforms.cpp
+  src/detail/frame_remapper.cpp
   src/image_publisher.cpp
   src/image_subscription.cpp
   src/node_handle.cpp
@@ -55,17 +56,11 @@ target_include_directories(
                          $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
 )
 target_link_libraries(
-  ${PROJECT_NAME}
-  PUBLIC CLI11::CLI11
-         message_filters::message_filters
-         rclcpp::rclcpp
+  ${PROJECT_NAME} PUBLIC message_filters::message_filters rclcpp::rclcpp
 )
-ament_target_dependencies(${PROJECT_NAME} PUBLIC sensor_msgs std_srvs)
+ament_target_dependencies(${PROJECT_NAME} PUBLIC sensor_msgs std_srvs tf2_msgs)
 
-add_library(${PROJECT_NAME}_rosbag
-  src/detail/bag_topic_checker.cpp
-  src/bag_reader.cpp
-)
+add_library(${PROJECT_NAME}_rosbag src/detail/bag_topic_checker.cpp src/bag_reader.cpp)
 target_include_directories(
   ${PROJECT_NAME}_rosbag PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
                                 $<INSTALL_INTERFACE:include/${PROJECT_NAME}>
@@ -76,14 +71,11 @@ target_link_libraries(
 
 # various plugins for executables to load
 add_library(
-  ${PROJECT_NAME}_plugins SHARED
-  src/app/signal_stop_plugin.cpp
-  src/app/tf_plugin.cpp
-  src/app/topic_remap_plugin.cpp
+  ${PROJECT_NAME}_plugins SHARED src/app/signal_stop_plugin.cpp src/app/tf_plugin.cpp
+                                 src/app/topic_remap_plugin.cpp
 )
 target_link_libraries(
-  ${PROJECT_NAME}_plugins
-  PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_rosbag
+  ${PROJECT_NAME}_plugins PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_rosbag CLI11::CLI11
   PRIVATE ament_shim
 )
 
@@ -94,36 +86,44 @@ if(NOT BUILD_SHARED_LIBS)
   set_property(TARGET ${PROJECT_NAME}_rosbag PROPERTY POSITION_INDEPENDENT_CODE ON)
 endif()
 
-#######################################################################################
+# ######################################################################################
 # Executables
-#######################################################################################
+# ######################################################################################
 
 add_executable(csv_to_tf app/csv_to_tf.cpp)
 ament_target_dependencies(csv_to_tf PUBLIC tf2_ros)
-target_link_libraries(csv_to_tf PUBLIC ${PROJECT_NAME})
+target_link_libraries(csv_to_tf PUBLIC ${PROJECT_NAME} CLI11::CLI11)
 
 add_executable(merge_rosbags app/merge_rosbags.cpp)
-target_link_libraries(merge_rosbags PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_rosbag)
+target_link_libraries(
+  merge_rosbags PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_rosbag CLI11::CLI11
+)
 
 add_executable(play_rosbag app/play_rosbag.cpp)
-target_link_libraries(play_rosbag PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_rosbag)
+target_link_libraries(
+  play_rosbag PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_rosbag CLI11::CLI11
+)
+
+add_executable(static_tfs_from_bag app/static_tfs_from_bag.cpp)
+target_link_libraries(
+  static_tfs_from_bag PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_rosbag yaml-cpp::yaml-cpp
+                             CLI11::CLI11
+)
 
 add_executable(transform_file_broadcaster app/transform_file_broadcaster.cpp)
 ament_target_dependencies(transform_file_broadcaster PUBLIC tf2_ros rclcpp)
-target_link_libraries(transform_file_broadcaster PUBLIC yaml-cpp::yaml-cpp)
+target_link_libraries(transform_file_broadcaster PUBLIC yaml-cpp::yaml-cpp CLI11::CLI11)
 
 add_executable(transform_file_lookup app/transform_file_lookup.cpp)
 ament_target_dependencies(transform_file_lookup PUBLIC tf2)
-target_link_libraries(transform_file_lookup PUBLIC yaml-cpp::yaml-cpp)
+target_link_libraries(transform_file_lookup PUBLIC yaml-cpp::yaml-cpp CLI11::CLI11)
 
-#######################################################################################
+# ######################################################################################
 # Bindings
-#######################################################################################
+# ######################################################################################
 
 pybind11_add_module(
-  _${PROJECT_NAME}_bindings
-  src/bindings/image_encodings.cpp
-  src/bindings/module.cpp
+  _${PROJECT_NAME}_bindings src/bindings/image_encodings.cpp src/bindings/module.cpp
 )
 target_include_directories(
   _${PROJECT_NAME}_bindings
@@ -135,22 +135,24 @@ ament_target_dependencies(_${PROJECT_NAME}_bindings PUBLIC sensor_msgs)
 include(CheckIPOSupported)
 check_ipo_supported(RESULT IPO_SUPPORTED)
 if(IPO_SUPPORTED)
-  set_property(TARGET _${PROJECT_NAME}_bindings PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE)
+  set_property(
+    TARGET _${PROJECT_NAME}_bindings PROPERTY INTERPROCEDURAL_OPTIMIZATION TRUE
+  )
 endif()
 
-#######################################################################################
+# ######################################################################################
 # Testing
-#######################################################################################
+# ######################################################################################
 
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   ament_add_gtest(${PROJECT_NAME}_test test/main.cpp test/test_bag_topic_checker.cpp)
   target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} ${PROJECT_NAME}_rosbag)
 endif()
 
-#######################################################################################
+# ######################################################################################
 # Installation
-#######################################################################################
+# ######################################################################################
 
 install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}/)
 install(
@@ -160,6 +162,7 @@ install(
           csv_to_tf
           merge_rosbags
           play_rosbag
+          static_tfs_from_bag
           transform_file_broadcaster
           transform_file_lookup
   EXPORT ${PROJECT_NAME}-exports
@@ -177,5 +180,5 @@ ament_python_install_package(${PROJECT_NAME})
 
 pluginlib_export_plugin_description_file(ianvs plugins.xml)
 ament_export_targets(${PROJECT_NAME}-exports HAS_LIBRARY_TARGET)
-ament_export_dependencies(message_filters rclcpp sensor_msgs std_srvs CLI11)
+ament_export_dependencies(message_filters rclcpp sensor_msgs std_srvs)
 ament_package()

ianvs/app/static_tfs_from_bag.cpp

@@ -0,0 +1,110 @@
+#include <yaml-cpp/yaml.h>
+
+#include <filesystem>
+
+#include <CLI/CLI.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/serialization.hpp>
+#include <rosbag2_transport/reader_writer_factory.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include "ianvs/detail/frame_remapper.h"
+
+struct AppArgs {
+  void add_to_app(CLI::App& app);
+
+  std::filesystem::path bag;
+  std::filesystem::path output;
+  ianvs::FrameRemapper::Config remapper;
+};
+
+void AppArgs::add_to_app(CLI::App& app) {
+  app.add_option("bag_path", bag)->required()->check(CLI::ExistingPath)->description("Bag to open");
+  app.add_option("-o,--output", output);
+  app.add_option("-p,--prefix-frames", remapper.prefix)
+      ->take_last()
+      ->description("prefix to apply to ALL frames");
+  app.add_option("-f,--filter-frames", remapper.filter)
+      ->join('|')
+      ->description("optional regex filter to drop frames (applied before prefix)");
+  app.add_option("-k,--keep-frames", remapper.keep)
+      ->join('|')
+      ->description("optional regex filter to keep frames (applied before prefix)");
+  app.add_option("-s,--frame-substitution", remapper.substitutions)
+      ->description("apply substitution to frames (match and substituion are separated by :)");
+  app.footer(R"(
+Both --filter-frames and --keep-frames support two match modes for a given
+TF. They can either be provided with a single regex that can match either
+the parent or the child frame OR they can be provided with two regexes in
+the form <PARENT_REGEX>:<CHILD_REGEX>, which requires that the parent frame
+matches the first regex and the child frame matches the second regex. This
+precludes the use of non-captured groups in the regex
+)");
+}
+
+int main(int argc, char** argv) {
+  CLI::App app("Utility to save static TFS from a file");
+  argv = app.ensure_utf8(argv);
+  app.allow_extras();
+  app.get_formatter()->column_width(50);
+
+  AppArgs args;
+  args.add_to_app(app);
+  try {
+    app.parse(argc, argv);
+  } catch (const CLI::ParseError& e) {
+    return app.exit(e);
+  }
+
+  args.bag = std::filesystem::canonical(args.bag);
+  rosbag2_storage::StorageOptions storage_opts;
+  storage_opts.uri = args.bag;
+  auto reader = rosbag2_transport::ReaderWriterFactory::make_reader(storage_opts);
+  if (!reader) {
+    return 1;
+  }
+
+  ianvs::FrameRemapper::PoseMap pose_map;
+  const ianvs::FrameRemapper remapper(args.remapper);
+
+  rosbag2_storage::StorageFilter filter;
+  filter.topics = {"/tf_static"};
+  const rclcpp::Serialization<tf2_msgs::msg::TFMessage> serialization;
+
+  reader->open(storage_opts);
+  reader->set_filter(filter);
+  while (reader->has_next()) {
+    const auto msg = reader->read_next();
+    rclcpp::SerializedMessage serialized_msg(*msg->serialized_data);
+    auto tf_msg = std::make_shared<tf2_msgs::msg::TFMessage>();
+    serialization.deserialize_message(&serialized_msg, tf_msg.get());
+    remapper.updatePoseMap(*tf_msg, pose_map);
+  }
+
+  YAML::Node root;
+  for (const auto& [parent, children] : pose_map) {
+    for (const auto& [child, pose] : children) {
+      YAML::Node tf;
+      tf["frame_id"] = parent;
+      tf["child_frame_id"] = child;
+      tf["x"] = pose.transform.translation.x;
+      tf["y"] = pose.transform.translation.y;
+      tf["z"] = pose.transform.translation.z;
+      tf["qw"] = pose.transform.rotation.w;
+      tf["qx"] = pose.transform.rotation.x;
+      tf["qy"] = pose.transform.rotation.y;
+      tf["qz"] = pose.transform.rotation.z;
+      root["frames"].push_back(tf);
+    }
+  }
+
+  std::filesystem::path output = args.output;
+  if (output.empty()) {
+    output = args.bag.parent_path() / (args.bag.stem().string() + "_static_tfs.yaml");
+  }
+
+  std::cout << "Saving " << root["frames"].size() << "transforms to " << output << std::endl;
+  std::ofstream fout(output);
+  fout << root;
+  return 0;
+}

ianvs/include/ianvs/detail/frame_remapper.h

@@ -0,0 +1,50 @@
+#pragma once
+#include <optional>
+
+#include <tf2_msgs/msg/tf_message.hpp>
+
+#include "ianvs/detail/string_transforms.h"
+
+namespace ianvs {
+
+struct FrameRemapper {
+ public:
+  using Transform = geometry_msgs::msg::TransformStamped;
+  using Msg = tf2_msgs::msg::TFMessage;
+  using PoseMap = std::map<std::string, std::map<std::string, Transform>>;
+
+  struct TFRegex {
+    explicit TFRegex(const std::string& arg);
+
+    bool matches(const std::string& parent_id, const std::string& child_id) const;
+
+    std::regex parent;
+    std::optional<std::regex> child;
+  };
+
+  struct Config {
+    std::string prefix;
+    std::vector<std::string> filter;
+    std::vector<std::string> keep;
+    std::vector<std::string> substitutions;
+  } const config;
+
+  explicit FrameRemapper(const Config& config, const rclcpp::Logger* logger = nullptr);
+
+  void updatePoseMap(const Msg& msg,
+                     PoseMap& pose_map,
+                     const rclcpp::Logger* logger = nullptr) const;
+
+ private:
+  std::string remapFrame(const std::string& frame_id) const;
+
+  bool shouldKeep(const std::string& parent, const std::string& child) const;
+
+  bool shouldFilter(const std::string& parent, const std::string& child) const;
+
+  std::vector<detail::StringTransform> transforms_;
+  std::vector<TFRegex> keep_;
+  std::vector<TFRegex> filter_;
+};
+
+}  // namespace ianvs