Feature/frame transformer node

frame_transformer/CMakeLists.txt

@@ -0,0 +1,81 @@
+
+# Copyright (C) 2021 LEIDOS.
+#
+# Licensed under the Apache License, Version 2.0 (the "License"); you may not
+# use this file except in compliance with the License. You may obtain a copy of
+# the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations under
+# the License.
+
+cmake_minimum_required(VERSION 3.5)
+project(frame_transformer)
+
+# Declare carma package and check ROS version
+find_package(carma_cmake_common REQUIRED)
+carma_check_ros_version(2)
+carma_package()
+
+## Find dependencies using ament auto
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Name build targets
+set(node_exec frame_transformer_node_exec)
+set(node_exec_auto_transition frame_transformer_node_exec_auto_transition)
+set(node_lib frame_transformer_node)
+
+# Includes
+include_directories(
+  include
+)
+
+# Build
+ament_auto_add_library(${node_lib} SHARED
+        src/frame_transformer_node.cpp
+)
+
+ament_auto_add_executable(${node_exec} 
+        src/lifecycle_main.cpp
+)
+
+ament_auto_add_executable(${node_exec_auto_transition} 
+        src/auto_transition_main.cpp
+)
+
+# Register component
+rclcpp_components_register_nodes(${node_lib} "frame_transformer::Node")
+
+# All locally created targets will need to be manually linked
+# ament auto will handle linking of external dependencies
+target_link_libraries(${node_exec}
+        ${node_lib}
+)
+
+target_link_libraries(${node_exec_auto_transition} 
+        ${node_lib}
+)
+
+# Testing
+if(BUILD_TESTING)  
+
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies() # This populates the ${${PROJECT_NAME}_FOUND_TEST_DEPENDS} variable
+
+  ament_add_gtest(test_frame_transformer test/node_test.cpp)
+
+  ament_target_dependencies(test_frame_transformer ${${PROJECT_NAME}_FOUND_TEST_DEPENDS})
+
+  target_link_libraries(test_frame_transformer ${node_lib})
+
+endif()
+
+# Install
+ament_auto_package(
+        INSTALL_TO_SHARE config launch
+)

frame_transformer/README.md

@@ -0,0 +1,3 @@
+# frame_transformer
+
+TODO for USER: Add description of package and link to confluence documentation.

frame_transformer/config/parameters.yaml

@@ -0,0 +1,11 @@
+# String: The target coordinate frame id to transform data into
+target_frame : "map"
+
+# String: Message type which will be transformed. A specialization of tf2_ros::Buffer::doTransform must be available for this type
+message_type : "sensor_msgs/PointCloud2"
+
+# Integer: Queue size for publishers and subscribers
+queue_size : 1
+
+# Integer: Timeout in ms for transform lookup. A value of 0 means lookup will occur once without blocking if it fails.
+timeout : 0

frame_transformer/include/frame_transformer/frame_transformer.hpp

@@ -0,0 +1,138 @@
+#pragma once
+
+/*
+ * Copyright (C) 2021 LEIDOS.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+#include "frame_transformer_base.hpp"
+#include <carma_ros2_utils/carma_lifecycle_node.hpp>
+#include <tf2/exceptions.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <chrono>
+
+namespace frame_transformer
+{
+  namespace std_ph = std::placeholders;
+  using std_ms = std::chrono::milliseconds;
+
+  /**
+   * \brief Class template for data transformers which use the tf2_ros::Buffer.transform() method to perform transforms on ros messages.
+   *        The class sets up publishers and subscribers using the provided node for the specified message type.
+   *        The specified message type must have a tf2_ros::Buffer.doTransform specialization for its type in order for it to compile.
+   * 
+   * \tparam The message type to transform. Must be supported by tf2_ros::Buffer.doTransform
+   */
+  template <class T>
+  class Transformer : public TransformerBase // TransformerBase allows for type polymorphism in the containing Node and constructor enforcement
+  {
+
+  protected:
+    // Subscribers
+    carma_ros2_utils::SubPtr<T> input_sub_;
+
+    // Publishers
+    carma_ros2_utils::PubPtr<T> output_pub_;
+
+
+  public:
+
+    /**
+     * \brief Constructor which sets up the required publishers and subscribers
+     * 
+     * See comments in TransformerBase for parameter descriptions
+     */ 
+    Transformer(Config config, std::shared_ptr<tf2_ros::Buffer> buffer, std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> node)
+    : TransformerBase(config, buffer, node) {
+
+      input_sub_ = node_->create_subscription<T>("input", config_.queue_size,
+                                                  std::bind(&Transformer::input_callback, this, std_ph::_1));
+
+      // Setup publishers
+      output_pub_ = node_->create_publisher<T>("output", config_.queue_size);
+    }
+
+
+    /**
+     * \brief Helper method which takes in an input message and transforms it to the provided frame.
+     *        Returns false if the provided timeout is exceeded for getting the transform or the transform could not be computed
+     * 
+     * \param in The input message to transform
+     * \param[out] out The preallocated location for the output message
+     * \param target_frame The frame the out message data will be in
+     * \param timeout A timeout in ms which if exceeded will result in a false return and invalid out value. This call may block for this period.
+     *                If set to zero, then lookup will be attempted only once. 
+     * 
+     * \return True if transform succeeded, false if timeout exceeded or transform could not be performed. 
+     */ 
+    bool transform(const T &in, T &out, const std::string &target_frame, const std_ms timeout)
+    {
+
+      try
+      {
+        buffer_->transform(in, out, target_frame, timeout);
+      }
+      catch (tf2::TransformException &ex)
+      {
+        std::string error = ex.what();
+        error = "Failed to get transform with exception: " + error;
+        auto& clk = *node_->get_clock(); // Separate reference required for proper throttle macro call
+        RCLCPP_WARN_THROTTLE(node_->get_logger(), clk, 1000, error);
+
+        return false;
+      }
+
+      return true;
+    }
+
+    /**
+     * \brief Callback for input data. Transforms the data then republishes it
+     * 
+     * NOTE: This method can be specialized for unique preallocation approaches for large messages such as point clouds or images
+     * 
+     * \param in_msg The input message to transform
+     */ 
+    void input_callback(std::unique_ptr<T> in_msg)
+    {
+      T out_msg;
+
+      if (!transform(*in_msg, out_msg, config_.target_frame, std_ms(config_.timeout)))
+      {
+        return;
+      }
+
+      output_pub_->publish(out_msg);
+    }
+  };
+
+  // Specialization of input_callback for PointCloud2 messages to allow for preallocation of points vector
+  // This is done due to the large size of that data set
+  template <>
+  inline void Transformer<sensor_msgs::msg::PointCloud2>::input_callback(std::unique_ptr<sensor_msgs::msg::PointCloud2> in_msg) {
+
+    sensor_msgs::msg::PointCloud2 out_msg;
+    out_msg.data.reserve(in_msg->data.size()); // Preallocate points vector
+
+    if (!transform(*in_msg, out_msg, config_.target_frame, std_ms(config_.timeout)))
+    {
+      return;
+    }
+
+    output_pub_->publish(out_msg);
+  }
+
+}

frame_transformer/include/frame_transformer/frame_transformer_base.hpp

@@ -0,0 +1,49 @@
+#pragma once
+/*
+ * Copyright (C) 2021 LEIDOS.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+#include <string>
+#include <memory>
+#include <tf2_ros/buffer.h>
+
+namespace frame_transformer
+{
+
+  /**
+   * \brief Transformer base class provides default member variables for tf2 lookup and ros network access. 
+   *        The constructor is also private meaning it must be called by extending classes in an attempt to enforce a construction interface.
+   * 
+   */
+  class TransformerBase
+  {
+
+  protected:
+
+    //! Transformation configuration
+    Config config_;
+
+    //! TF2 Buffer for storing transform history and looking up transforms
+    std::shared_ptr<tf2_ros::Buffer> buffer_;
+
+    //! Containing node which provides access to the ros network
+    std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> node_;
+
+  protected: // Protected to force super call in child classes
+    TransformerBase(const Config& config, std::shared_ptr<tf2_ros::Buffer> buffer, std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> node) 
+      : config_(config), buffer_(buffer), node_(node) {}
+  };
+
+}

frame_transformer/include/frame_transformer/frame_transformer_config.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+/*
+ * Copyright (C) 2021 LEIDOS.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not
+ * use this file except in compliance with the License. You may obtain a copy of
+ * the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations under
+ * the License.
+ */
+
+#include <iostream>
+
+namespace frame_transformer
+{
+
+  /**
+   * \brief Stuct containing the algorithm configuration values for frame_transformer
+   */
+  struct Config
+  {
+    //! The target coordinate frame id to transform data into
+    std::string target_frame = "map";
+
+    //! Message type which will be transformed
+    std::string message_type = "sensor_msgs/PointCloud2";
+
+    //! Queue size for publishers and subscribers
+    int queue_size = 1;
+
+    //! Timeout in ms for transform lookup. A value of 0 means lookup will occur once without blocking if it fails.
+    int timeout = 0;
+
+    // Stream operator for this config
+    friend std::ostream &operator<<(std::ostream &output, const Config &c)
+    {
+      output << "frame_transformer::Config { " << std::endl
+           << "target_frame: " << c.target_frame << std::endl
+           << "message_type: " << c.message_type << std::endl
+           << "queue_size: " << c.queue_size << std::endl
+           << "timeout: " << c.timeout << std::endl
+           << "}" << std::endl;
+      return output;
+    }
+  };
+
+} // frame_transformer