Cleanup misc

bitbots_utils/CMakeLists.txt

@@ -1,35 +1,57 @@
 cmake_minimum_required(VERSION 3.5)
 project(bitbots_utils)
 
+# Add support for C++17
+if (NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif ()
+
 find_package(bitbots_docs REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
+enable_bitbots_docs()
 
+# Define include directories for cpp library headers
 set(INCLUDE_DIRS include)
 include_directories(${INCLUDE_DIRS})
 
-set(CMAKE_CXX_STANDARD 17)
 add_compile_options(-Wall -Werror -Wno-unused -pedantic -Wextra)
 
-enable_bitbots_docs()
+# Add cpp library
+add_library(${PROJECT_NAME} src/utils.cpp)
 
-add_executable(tf_delay_plot scripts/tf_delay_plot.cpp)
+# Add dependencies to cpp library
+ament_target_dependencies(${PROJECT_NAME}
+                          rclcpp
+                          tf2_ros)
 
-ament_target_dependencies(tf_delay_plot
-        ament_cmake
-        rclcpp
-        tf2_msgs
-        std_msgs)
+ament_export_dependencies(rclcpp)   
+ament_export_include_directories(${INCLUDE_DIRS})
+ament_export_libraries(${PROJECT_NAME})
 
+# Python install
 ament_python_install_package(${PROJECT_NAME})
 
+# CPP Script install
+add_executable(tf_delay_plot scripts/tf_delay_plot.cpp)
+
+ament_target_dependencies(tf_delay_plot
+                          ament_cmake
+                          rclcpp
+                          tf2_msgs
+                          std_msgs)
+
 install(TARGETS tf_delay_plot
         DESTINATION lib/${PROJECT_NAME})
 
+
+
+# Copy config and launch files
 install(DIRECTORY config
   DESTINATION share/${PROJECT_NAME})
 
@@ -40,4 +62,13 @@ install(DIRECTORY scripts/
   USE_SOURCE_PERMISSIONS
   DESTINATION lib/${PROJECT_NAME})
 
+# Install library  
+install(DIRECTORY include/ 
+        DESTINATION include)
+
+install(TARGETS ${PROJECT_NAME}
+        EXPORT ${PROJECT_NAME}
+        LIBRARY DESTINATION lib
+        INCLUDES DESTINATION include)
+
 ament_package()

bitbots_utils/include/bitbots_utils/utils.hpp

@@ -0,0 +1,40 @@
+#ifndef BITBOTS_UTILS__UTILS_H_
+#define BITBOTS_UTILS__UTILS_H_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <string>
+#include <tf2_ros/buffer.h>
+#include <vector> 
+
+using namespace std::chrono_literals;
+
+namespace bitbots_utils
+{
+
+/**
+ * @brief Waits for the transforms to be available
+ * @param logger The logger to use for logging
+ * @param clock The clock to use for time
+ * @param tf_buffer The tf buffer to use
+ * @param frames The tf frames to wait for
+ * @param check_interval Interval in which to check for the frames
+ * @param warn_duration Duration after which to warn if the frames are not available
+ * @param warn_interval Interval in which to keep warning if the frames are not available
+ * @param verbose Can be used to disable the warning messages
+ */
+void wait_for_tf(
+    const rclcpp::Logger &logger, 
+    std::shared_ptr<rclcpp::Clock> clock, 
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer, 
+    const std::vector<std::string> &frames, 
+    const std::string &root_frame,
+    const rclcpp::Duration &check_interval = rclcpp::Duration(0.1s),
+    const rclcpp::Duration &warn_duration = rclcpp::Duration(5.0s),
+    const rclcpp::Duration &warn_interval = rclcpp::Duration(1.0s),
+    bool verbose = true);
+
+}  // namespace bitbots_utils
+
+#endif  // BITBOTS_UTILS__UTILS_H_

bitbots_utils/src/utils.cpp

@@ -0,0 +1,80 @@
+#include "bitbots_utils/utils.hpp"
+
+namespace bitbots_utils
+{
+
+/**
+ * @brief Waits for the transforms to be available
+ * @param logger The logger to use for logging
+ * @param clock The clock to use for time
+ * @param tf_buffer The tf buffer to use
+ * @param frames The tf frames to wait for
+ * @param check_interval Interval in which to check for the frames
+ * @param warn_duration Duration after which to warn if the frames are not available
+ * @param warn_interval Interval in which to keep warning if the frames are not available
+ * @param verbose Can be used to disable the warning messages
+ */
+void wait_for_tf(
+    const rclcpp::Logger &logger,
+    std::shared_ptr<rclcpp::Clock> clock,
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer,
+    const std::vector<std::string> &frames, 
+    const std::string &root_frame,
+    const rclcpp::Duration &check_interval, 
+    const rclcpp::Duration &warn_duration, 
+    const rclcpp::Duration &warn_interval, 
+    bool verbose) {
+
+  // Store the beginning time
+  auto start_time = clock->now();
+
+  // Endless loop with increasing counter
+  while(rclcpp::ok()) {
+    try {
+      // Check if the frame we want to transform is known yet
+      // Apply tf_buffer->_frameExists to all frames and check if all are available otherwise return false (functional)
+      // We use _frameExists because it does not spam the console with errors if the frame does not exist...
+      if(!std::all_of(frames.begin(), frames.end(), [tf_buffer](std::string frame) {return tf_buffer->_frameExists(frame);})) {
+        // Don't do busy waiting
+        rclcpp::sleep_for(check_interval.to_chrono<std::chrono::nanoseconds>());
+        // Retry
+        continue;
+      }
+
+      // Check if we can transform from base_link to all given frames
+      if(!std::all_of(
+        frames.begin(), 
+        frames.end(), 
+        [tf_buffer, root_frame, check_interval](const std::string frame) {
+          return tf_buffer->canTransform(
+            root_frame,
+            frame, 
+            rclcpp::Time(0), 
+            check_interval);
+        })){
+        // Here it is fine not to wait as the canTransform function already includes a timeout
+        // Retry
+        continue;
+      }
+      // We can transform to all frames, so we are done
+      return;
+    }
+    catch(const std::exception& e)
+    {
+      if (verbose) {
+        RCLCPP_ERROR(logger, "Error while waiting for transforms: %s \n", e.what());
+      }
+      rclcpp::sleep_for(check_interval.to_chrono<std::chrono::nanoseconds>());
+    }
+
+    // Print error message if we waited too long
+    auto wait_time = clock->now() - start_time;
+    if (verbose && wait_time > warn_duration) {
+      RCLCPP_WARN_THROTTLE(logger, *clock, warn_interval.seconds(),
+                          "Waiting for transforms took longer than %f seconds", 
+                          wait_time.seconds());
+    }
+  }
+}
+
+}