Common: decouple tf and ros

modules/common/adapters/BUILD

@@ -29,6 +29,7 @@ cc_library(
         "//modules/common",
         "//modules/common/adapters/proto:adapter_config_proto",
         "//modules/common/monitor/proto:monitor_proto",
+        "//modules/common/transform_listener",
         "//modules/common/util",
         "@glog//:glog",
         "@ros//:ros_common",

modules/common/adapters/adapter_manager.h

@@ -32,9 +32,9 @@
 #include "modules/common/adapters/proto/adapter_config.pb.h"
 #include "modules/common/log.h"
 #include "modules/common/macro.h"
+#include "modules/common/transform_listener/transform_listener.h"
 
 #include "ros/include/ros/ros.h"
-#include "tf/transform_listener.h"
 
 /**
  * @namespace apollo::common::adapter
@@ -115,7 +115,9 @@ namespace adapter {
                                                                                \
     observers_.push_back([this]() { name##_->Observe(); });                    \
   }                                                                            \
-  name##Adapter *InternalGet##name() { return name##_.get(); }                 \
+  name##Adapter *InternalGet##name() {                                         \
+    return name##_.get();                                                      \
+  }                                                                            \
   void InternalPublish##name(const name##Adapter::DataType &data) {            \
     /* Only publish ROS msg if node handle is initialized. */                  \
     if (IsRos()) {                                                             \
@@ -183,14 +185,17 @@ class AdapterManager {
   /**
    * @brief Returns whether AdapterManager is running ROS mode.
    */
-  static bool IsRos() { return instance()->node_handle_ != nullptr; }
+  static bool IsRos() {
+    return instance()->node_handle_ != nullptr;
+  }
 
   /**
    * @brief Returns a reference to static tf2 buffer.
    */
   static tf2_ros::Buffer &Tf2Buffer() {
     static tf2_ros::Buffer tf2_buffer;
-    static tf2_ros::TransformListener tf2Listener(tf2_buffer);
+    static TransformListener tf2Listener(&tf2_buffer,
+                                         instance()->node_handle_.get());
     return tf2_buffer;
   }
 

modules/common/transform_listener/BUILD

+load("//tools:cpplint.bzl", "cpplint")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "transform_listener",
+    hdrs = [
+        "transform_listener.h",
+    ],
+    srcs = [
+        "transform_listener.cc",
+    ],
+    deps = [
+        "//modules/common:log",
+        "//modules/common/time",
+        "@ros//:ros_common",
+    ],
+)
+
+cpplint()

modules/common/transform_listener/transform_listener.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * 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 "modules/common/transform_listener/transform_listener.h"
+
+#include <functional>
+
+#include "modules/common/log.h"
+#include "modules/common/time/time.h"
+
+namespace apollo {
+namespace common {
+
+using apollo::common::time::Clock;
+using tf2_msgs::TFMessage;
+using std::placeholders::_1;
+
+TransformListener::TransformListener(tf2::BufferCore* buffer,
+                                     ros::NodeHandle* nh, bool spin_thread)
+    : node_(nh),
+      buffer_(buffer),
+      using_dedicated_thread_(spin_thread),
+      last_update_(0.0) {
+  if (node_ == nullptr) {
+    // There is no node handle when ROS is not enabled.
+    return;
+  }
+  if (spin_thread) {
+    InitWithThread();
+  } else {
+    Init();
+  }
+}
+
+TransformListener::~TransformListener() {
+  using_dedicated_thread_ = false;
+  if (dedicated_listener_thread_) {
+    dedicated_listener_thread_->join();
+  }
+}
+
+void TransformListener::Init() {
+  tf_subscriber_ =
+      node_->subscribe("/tf", kQueueSize, &TransformListener::TfCallback, this);
+  tf_static_subscriber_ = node_->subscribe(
+      "/tf_static", kQueueSize, &TransformListener::TfStaticCallback, this);
+}
+
+void TransformListener::InitWithThread() {
+  using_dedicated_thread_ = true;
+  ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<TFMessage>(
+      "/tf", kQueueSize, std::bind(&TransformListener::TfCallback, this, _1),
+      ros::VoidPtr(), &tf_message_callback_queue_);
+  tf_subscriber_ = node_->subscribe(ops_tf);
+
+  ros::SubscribeOptions ops_tf_static =
+      ros::SubscribeOptions::create<TFMessage>(
+          "/tf_static", kQueueSize,
+          std::bind(&TransformListener::TfStaticCallback, this, _1),
+          ros::VoidPtr(), &tf_message_callback_queue_);
+  tf_static_subscriber_ = node_->subscribe(ops_tf_static);
+
+  dedicated_listener_thread_.reset(new std::thread(
+      std::bind(&TransformListener::DedicatedListenerThread, this)));
+
+  // Tell the buffer we have a dedicated thread to enable timeouts
+  buffer_->setUsingDedicatedThread(true);
+}
+
+void TransformListener::TfCallback(const tf2_msgs::TFMessage::ConstPtr& tf) {
+  CallbackImpl(tf, false);
+}
+void TransformListener::TfStaticCallback(
+    const tf2_msgs::TFMessage::ConstPtr& tf_static) {
+  CallbackImpl(tf_static, true);
+}
+
+void TransformListener::CallbackImpl(const tf2_msgs::TFMessage::ConstPtr& tf,
+                                     bool is_static) {
+  double now = Clock::NowInSecond();
+  if (now < last_update_) {
+    AWARN << "Detected jump back in time. Clearing TF buffer.";
+    buffer_->clear();
+  }
+  last_update_ = now;
+  for (size_t i = 0; i < tf->transforms.size(); i++) {
+    try {
+      buffer_->setTransform(tf->transforms[i], "tf", is_static);
+    } catch (tf2::TransformException& ex) {
+      AERROR << "Failure to set recieved transform from "
+             << tf->transforms[i].child_frame_id << " to "
+             << tf->transforms[i].header.frame_id
+             << " with error: " << ex.what();
+    }
+  }
+}
+
+void TransformListener::DedicatedListenerThread() {
+  while (using_dedicated_thread_) {
+    tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
+  }
+}
+
+}  // namespace common
+}  // namespace apollo

modules/common/transform_listener/transform_listener.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * 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.
+ *****************************************************************************/
+
+#ifndef MODULES_COMMON_TRANSFORM_LISTENER_
+#define MODULES_COMMON_TRANSFORM_LISTENER_
+
+#include <memory>
+#include <thread>
+
+#include "ros/include/ros/callback_queue.h"
+#include "ros/include/ros/ros.h"
+#include "ros/include/tf2_msgs/TFMessage.h"
+#include "ros/include/tf2_ros/buffer.h"
+
+/**
+ * @namespace apollo::common
+ * @brief apollo::common
+ */
+namespace apollo {
+namespace common {
+
+class TransformListener {
+ public:
+  TransformListener(tf2::BufferCore* buffer, ros::NodeHandle* nh,
+                    bool spin_thread = true);
+
+  ~TransformListener();
+
+  /// Callback function for ros message subscription
+  void TfCallback(const tf2_msgs::TFMessage::ConstPtr& tf);
+  void TfStaticCallback(const tf2_msgs::TFMessage::ConstPtr& tf_static);
+
+ private:
+  void Init();
+  void InitWithThread();
+
+  void DedicatedListenerThread();
+
+  void CallbackImpl(const tf2_msgs::TFMessage::ConstPtr& tf, bool is_static);
+
+  ros::CallbackQueue tf_message_callback_queue_;
+  std::unique_ptr<std::thread> dedicated_listener_thread_;
+  ros::NodeHandle* node_;  // Doesn't own NodeHandle
+  ros::Subscriber tf_subscriber_;
+  ros::Subscriber tf_static_subscriber_;
+  tf2::BufferCore* buffer_;
+  bool using_dedicated_thread_;
+  double last_update_;
+
+  static constexpr int kQueueSize = 100;
+};
+
+}  // namespace common
+}  // namespace apollo
+
+#endif  // MODULES_COMMON_TRANSFORM_LISTENER_