From 39995b404607ddb9e11b7e2aba3c05ce410b4e21 Mon Sep 17 00:00:00 2001
From: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp>
Date: Tue, 10 Dec 2024 12:00:50 +0900
Subject: [PATCH 1/2] feat: add periodic_transfrom_publisher

Signed-off-by: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp>
---
 .../CMakeLists.txt                            |  15 +
 .../broadcaster.hpp                           |  87 ++++
 src/periodic_transform_publisher/package.xml  |  23 ++
 .../src/periodic_transform_publisher.cpp      | 387 ++++++++++++++++++
 4 files changed, 512 insertions(+)
 create mode 100644 src/periodic_transform_publisher/CMakeLists.txt
 create mode 100644 src/periodic_transform_publisher/include/periodic_transform_publisher/broadcaster.hpp
 create mode 100644 src/periodic_transform_publisher/package.xml
 create mode 100644 src/periodic_transform_publisher/src/periodic_transform_publisher.cpp

diff --git a/src/periodic_transform_publisher/CMakeLists.txt b/src/periodic_transform_publisher/CMakeLists.txt
new file mode 100644
index 0000000..924d3b0
--- /dev/null
+++ b/src/periodic_transform_publisher/CMakeLists.txt
@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(periodic_transform_publisher)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_executable(${PROJECT_NAME}
+  src/periodic_transform_publisher.cpp
+)
+
+ament_auto_package()
diff --git a/src/periodic_transform_publisher/include/periodic_transform_publisher/broadcaster.hpp b/src/periodic_transform_publisher/include/periodic_transform_publisher/broadcaster.hpp
new file mode 100644
index 0000000..39c1cfb
--- /dev/null
+++ b/src/periodic_transform_publisher/include/periodic_transform_publisher/broadcaster.hpp
@@ -0,0 +1,87 @@
+#ifndef PERIODIC_TRANSFORM_PUBLISHER__BROADCASTER_HPP_
+#define PERIODIC_TRANSFORM_PUBLISHER__BROADCASTER_HPP_
+/*
+ * ref:
+ * https://github.com/ros2/geometry2/blob/humble/tf2_ros/src/static_transform_broadcaster_node.cpp
+ */
+
+#include <memory>
+#include <random>
+#include <string>
+
+#include "rclcpp/rclcpp.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+std::string get_unique_node_name()
+{
+  static const std::string chars = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
+  static std::random_device rd;
+  static std::minstd_rand g{rd()};
+
+  // The uniform_int_distribution takes a closed interval of [a, b]; since that
+  // would include the \0, we remove one from our string length.
+  static std::uniform_int_distribution<std::string::size_type> pick(0, chars.length() - 1);
+
+  std::string s{"periodic_transform_publisher_"};
+
+  size_t orig_length = s.length();
+  s.resize(orig_length + 16);
+
+  for (size_t i = orig_length; i < s.length(); ++i) {
+    s[i] = chars[pick(g)];
+  }
+
+  return s;
+}
+
+class Broadcaster : public rclcpp::Node
+{
+ public:
+  explicit Broadcaster(const rclcpp::NodeOptions & options)
+      : rclcpp::Node(get_unique_node_name(), options) {
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.read_only = true;
+
+    tf_msg_.transform.translation.x = this->declare_parameter("translation.x", 0.0, descriptor);
+    tf_msg_.transform.translation.y = this->declare_parameter("translation.y", 0.0, descriptor);
+    tf_msg_.transform.translation.z = this->declare_parameter("translation.z", 0.0, descriptor);
+    tf_msg_.transform.rotation.x = this->declare_parameter("rotation.x", 0.0, descriptor);
+    tf_msg_.transform.rotation.y = this->declare_parameter("rotation.y", 0.0, descriptor);
+    tf_msg_.transform.rotation.z = this->declare_parameter("rotation.z", 0.0, descriptor);
+    tf_msg_.transform.rotation.w = this->declare_parameter("rotation.w", 1.0, descriptor);
+    tf_msg_.header.frame_id = this->declare_parameter("frame_id", std::string("/frame"), descriptor);
+    tf_msg_.child_frame_id =
+        this->declare_parameter("child_frame_id", std::string("/child"), descriptor);
+
+    double interval_sec = this->declare_parameter("interval_sec", 1.0, descriptor);
+
+    // check frame_id != child_frame_id
+    if (tf_msg_.header.frame_id == tf_msg_.child_frame_id) {
+      RCLCPP_ERROR(
+          this->get_logger(),
+          "cannot publish transform from %s to %s, exiting",
+          tf_msg_.header.frame_id.c_str(), tf_msg_.child_frame_id.c_str()
+      );
+      throw std::runtime_error("child_frame_id cannot equal frame_id");
+    }
+
+    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+    timer_ = rclcpp::create_timer(this, get_clock(), rclcpp::Duration::from_seconds(interval_sec),
+                                  std::bind(&Broadcaster::callback, this));
+  }
+
+  void callback() {
+    tf_msg_.header.stamp = this->get_clock()->now();
+    tf_broadcaster_->sendTransform(tf_msg_);
+  }
+
+ protected:
+  geometry_msgs::msg::TransformStamped tf_msg_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+};
+
+#endif // PERIODIC_TRANSFORM_PUBLISHER__BROADCASTER_HPP_
diff --git a/src/periodic_transform_publisher/package.xml b/src/periodic_transform_publisher/package.xml
new file mode 100644
index 0000000..89cebe3
--- /dev/null
+++ b/src/periodic_transform_publisher/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>periodic_transform_publisher</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="manato.hirabayashi@tier4.jp">manatohirabayashi</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/src/periodic_transform_publisher/src/periodic_transform_publisher.cpp b/src/periodic_transform_publisher/src/periodic_transform_publisher.cpp
new file mode 100644
index 0000000..0eb70fc
--- /dev/null
+++ b/src/periodic_transform_publisher/src/periodic_transform_publisher.cpp
@@ -0,0 +1,387 @@
+/*
+ * This publisher based on the implementation of tf2_ros::static_transform_publisher.
+ * ref:
+ * https://github.com/ros2/geometry2/blob/humble/tf2_ros/src/static_transform_broadcaster_program.cpp
+ */
+
+#include <cstdio>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Vector3.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "periodic_transform_publisher/broadcaster.hpp"
+
+struct Option
+{
+  explicit Option(bool has_arg)
+  : has_argument(has_arg)
+  {
+  }
+
+  bool has_argument{false};
+
+  virtual std::string visit(const std::string & optname, const std::string & stringval) = 0;
+
+  virtual ~Option() {}
+};
+
+struct DoubleOption final : Option
+{
+  explicit DoubleOption(bool has_arg, std::function<void(double)> cb)
+  : Option(has_arg),
+    callback(cb)
+  {
+  }
+
+  std::function<void(double)> callback;
+
+  std::string visit(const std::string & optname, const std::string & stringval) override
+  {
+    double value;
+    try {
+      value = std::stod(stringval);
+    } catch (const std::invalid_argument &) {
+      return "Failed to parse " + optname + " argument as float";
+    }
+
+    callback(value);
+
+    return "";
+  }
+};
+
+struct StringOption final : Option
+{
+  explicit StringOption(bool has_arg, std::function<void(const std::string &)> cb)
+  : Option(has_arg),
+    callback(cb)
+  {
+  }
+
+  std::function<void(const std::string &)> callback;
+
+  std::string visit(const std::string & optname, const std::string & stringval) override
+  {
+    (void)optname;
+    callback(stringval);
+    return "";
+  }
+};
+
+static std::string parse_args(
+  const std::vector<std::string> & args,
+  bool & help,
+  tf2::Quaternion & quat,
+  tf2::Vector3 & trans,
+  std::string & frame_id,
+  std::string & child_frame_id)
+{
+  size_t size = args.size();
+
+  help = false;
+
+  if (size < 1) {
+    return "Not enough arguments to parse";
+  }
+
+  size_t last_index = size - 1;
+
+  bool saw_frame_flag = false;
+  bool saw_quat_flag = false;
+  bool saw_rpy_flag = false;
+  bool saw_trans_flag = false;
+  double roll = 0.0;
+  double pitch = 0.0;
+  double yaw = 0.0;
+
+  auto qx_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setX(value);
+      saw_quat_flag = true;
+    });
+
+  auto qy_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setY(value);
+      saw_quat_flag = true;
+    });
+
+  auto qz_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setZ(value);
+      saw_quat_flag = true;
+    });
+
+  auto qw_opt = std::make_shared<DoubleOption>(
+    true, [&quat, &saw_quat_flag](double value) {
+      quat.setW(value);
+      saw_quat_flag = true;
+    });
+
+  auto roll_opt = std::make_shared<DoubleOption>(
+    true, [&roll, &saw_rpy_flag](double value) {
+      roll = value;
+      saw_rpy_flag = true;
+    });
+
+  auto pitch_opt = std::make_shared<DoubleOption>(
+    true, [&pitch, &saw_rpy_flag](double value) {
+      pitch = value;
+      saw_rpy_flag = true;
+    });
+
+  auto yaw_opt = std::make_shared<DoubleOption>(
+    true, [&yaw, &saw_rpy_flag](double value) {
+      yaw = value;
+      saw_rpy_flag = true;
+    });
+
+  auto trans_x_opt = std::make_shared<DoubleOption>(
+    true, [&trans, &saw_trans_flag](double value) {
+      trans.setX(value);
+      saw_trans_flag = true;
+    });
+
+  auto trans_y_opt = std::make_shared<DoubleOption>(
+    true, [&trans, &saw_trans_flag](double value) {
+      trans.setY(value);
+      saw_trans_flag = true;
+    });
+
+  auto trans_z_opt = std::make_shared<DoubleOption>(
+    true, [&trans, &saw_trans_flag](double value) {
+      trans.setZ(value);
+      saw_trans_flag = true;
+    });
+
+  auto frame_id_opt = std::make_shared<StringOption>(
+    true, [&frame_id, &saw_frame_flag](const std::string & value) {
+      frame_id = value;
+      saw_frame_flag = true;
+    });
+
+  auto child_frame_id_opt = std::make_shared<StringOption>(
+    true, [&child_frame_id, &saw_frame_flag](const std::string & value) {
+      child_frame_id = value;
+      saw_frame_flag = true;
+    });
+
+  auto help_opt = std::make_shared<StringOption>(
+    false, [&help](const std::string & value) {
+      (void)value;
+      help = true;
+    });
+
+  std::unordered_map<std::string, std::shared_ptr<Option>> options = {
+    {"--qx", qx_opt},
+    {"--qy", qy_opt},
+    {"--qz", qz_opt},
+    {"--qw", qw_opt},
+    {"--roll", roll_opt},
+    {"--pitch", pitch_opt},
+    {"--yaw", yaw_opt},
+    {"--x", trans_x_opt},
+    {"--y", trans_y_opt},
+    {"--z", trans_z_opt},
+    {"--frame-id", frame_id_opt},
+    {"--child-frame-id", child_frame_id_opt},
+    {"--help", help_opt},
+    {"-h", help_opt},
+  };
+
+  std::vector<std::string> no_flag_args;
+
+  size_t i = 1;
+  while (i < size) {
+    const std::string & optname = args[i];
+    if (options.count(optname) == 0) {
+      no_flag_args.push_back(optname);
+    } else {
+      std::shared_ptr<Option> opt = options[optname];
+      if (opt->has_argument) {
+        if (i == last_index) {
+          return "Not enough arguments for " + optname;
+        }
+
+        ++i;
+      }
+      std::string result = opt->visit(optname, args[i]);
+      if (result != "") {
+        return result;
+      }
+    }
+    ++i;
+  }
+
+  if (help) {
+    return "";
+  }
+
+  if (saw_rpy_flag && saw_quat_flag) {
+    return "Cannot specify both quaternion and Euler rotations";
+  } else if (saw_rpy_flag) {
+    quat.setRPY(roll, pitch, yaw);
+  }
+
+  if (no_flag_args.size() == 8 || no_flag_args.size() == 9) {
+    RCUTILS_LOG_WARN("Old-style arguments are deprecated; see --help for new-style arguments");
+    if (saw_frame_flag || saw_trans_flag || saw_quat_flag) {
+      return "Cannot specify both new-style (flags) and old-style (arguments)";
+    }
+
+    std::string ret;
+
+    ret = trans_x_opt->visit("x", no_flag_args[0]);
+    if (ret != "") {
+      return ret;
+    }
+    ret = trans_y_opt->visit("y", no_flag_args[1]);
+    if (ret != "") {
+      return ret;
+    }
+    ret = trans_z_opt->visit("z", no_flag_args[2]);
+    if (ret != "") {
+      return ret;
+    }
+
+    if (no_flag_args.size() == 8) {
+      ret = yaw_opt->visit("yaw", no_flag_args[3]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = pitch_opt->visit("pitch", no_flag_args[4]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = roll_opt->visit("roll", no_flag_args[5]);
+      if (ret != "") {
+        return ret;
+      }
+
+      quat.setRPY(roll, pitch, yaw);
+      frame_id = no_flag_args[6];
+      child_frame_id = no_flag_args[7];
+    } else {
+      ret = qx_opt->visit("qx", no_flag_args[3]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = qy_opt->visit("qy", no_flag_args[4]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = qz_opt->visit("qz", no_flag_args[5]);
+      if (ret != "") {
+        return ret;
+      }
+      ret = qw_opt->visit("qw", no_flag_args[6]);
+      if (ret != "") {
+        return ret;
+      }
+      frame_id = no_flag_args[7];
+      child_frame_id = no_flag_args[8];
+    }
+  } else if (no_flag_args.size() != 0) {
+    return "Extra unparsed arguments on command-line";
+  }
+
+  if (frame_id == "") {
+    return "Frame id must not be empty";
+  }
+
+  if (child_frame_id == "") {
+    return "Child frame id must not be empty";
+  }
+
+  return "";
+}
+
+static void print_usage()
+{
+  const char * usage =
+    "usage: periodic_transform_publisher [--x X] [--y Y] [--z Z] [--qx QX] [--qy QY] "
+    "[--qz QZ] [--qw QW] [--roll ROLL] [--pitch PITCH] [--yaw YAW] --frame-id FRAME_ID "
+    "--child-frame-id CHILD_FRAME_ID\n\n"
+    "A command line utility for manually sending a static transform periodically.\n\nIf no translation or"
+    " orientation is provided, the identity transform will be published.\n\nThe translation offsets"
+    " are in meters.\n\nThe rotation may be provided with roll, pitch, yaw euler angles in radians,"
+    " or as a quaternion.\n\n"
+    "required arguments:\n"
+    "  --frame-id FRAME_ID parent frame\n"
+    "  --child-frame-id CHILD_FRAME_ID child frame id\n\n"
+    "optional arguments:\n"
+    "  --x X                 x component of translation\n"
+    "  --y Y                 y component of translation\n"
+    "  --z Z                 z component of translation\n"
+    "  --qx QX               x component of quaternion rotation\n"
+    "  --qy QY               y component of quaternion rotation\n"
+    "  --qz QZ               z component of quaternion rotation\n"
+    "  --qw QW               w component of quaternion rotation\n"
+    "  --roll ROLL           roll component Euler rotation\n"
+    "  --pitch PITCH         pitch component Euler rotation\n"
+    "  --yaw YAW             yaw component Euler rotation";
+  printf("%s\n", usage);
+}
+
+int main(int argc, char ** argv)
+{
+  // Initialize ROS
+  std::vector<std::string> args = rclcpp::init_and_remove_ros_arguments(argc, argv);
+  bool help = false;
+  tf2::Quaternion rotation(0.0, 0.0, 0.0, 1.0);
+  tf2::Vector3 translation(0.0, 0.0, 0.0);
+  std::string frame_id;
+  std::string child_frame_id;
+
+  std::string ret = parse_args(args, help, rotation, translation, frame_id, child_frame_id);
+  if (ret != "") {
+    RCUTILS_LOG_ERROR("error parsing command line arguments: %s", ret.c_str());
+    print_usage();
+    return 1;
+  }
+  if (help) {
+    print_usage();
+    return 0;
+  }
+
+  rclcpp::NodeOptions options;
+  // override default parameters with the desired transform
+  options.parameter_overrides(
+  {
+    {"translation.x", translation.x()},
+    {"translation.y", translation.y()},
+    {"translation.z", translation.z()},
+    {"rotation.x", rotation.x()},
+    {"rotation.y", rotation.y()},
+    {"rotation.z", rotation.z()},
+    {"rotation.w", rotation.w()},
+    {"frame_id", frame_id},
+    {"child_frame_id", child_frame_id},
+  });
+
+  std::shared_ptr<Broadcaster> node;
+
+  node = std::make_shared<Broadcaster>(options);
+
+  RCLCPP_INFO(
+    node->get_logger(),
+    "Spinning until stopped - publishing transform\ntranslation: ('%lf', '%lf', '%lf')\n"
+    "rotation: ('%lf', '%lf', '%lf', '%lf')\nfrom '%s' to '%s'",
+    translation.x(), translation.y(), translation.z(),
+    rotation.x(), rotation.y(), rotation.z(), rotation.w(),
+    frame_id.c_str(), child_frame_id.c_str());
+
+  rclcpp::spin(node);
+
+  rclcpp::shutdown();
+
+  return 0;
+}

From 77c8f4868303029e56ae22cc723333ab254eb0b2 Mon Sep 17 00:00:00 2001
From: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp>
Date: Wed, 11 Dec 2024 16:51:48 +0900
Subject: [PATCH 2/2] feat: use unified TF launcher to broadcast TF info
 periodically

Signed-off-by: Manato HIRABAYASHI <manato.hirabayashi@tier4.jp>
---
 .../launch/component/drs_camera.launch.xml    |   9 +-
 ...isher.launch.py => tf_publisher.launch.py} |  56 ++++++---
 src/drs_launch/launch/drs_ecu0.launch.xml     |  24 ++++
 src/drs_launch/launch/drs_ecu1.launch.xml     |  18 +++
 src/drs_launch/launch/drs_offline.launch.xml  | 112 ++++++++++++------
 src/drs_launch/package.xml                    |   1 +
 .../default/base_link_to_drs_base_link.yaml   |   9 ++
 .../default/drs_base_link_to_lidars.yaml      |  30 +++++
 8 files changed, 206 insertions(+), 53 deletions(-)
 rename src/drs_launch/launch/component/{lidar_camera_tf_publisher.launch.py => tf_publisher.launch.py} (64%)
 create mode 100644 src/individual_params/config/default/base_link_to_drs_base_link.yaml
 create mode 100644 src/individual_params/config/default/drs_base_link_to_lidars.yaml

diff --git a/src/drs_launch/launch/component/drs_camera.launch.xml b/src/drs_launch/launch/component/drs_camera.launch.xml
index 6f15de0..c72fc67 100644
--- a/src/drs_launch/launch/component/drs_camera.launch.xml
+++ b/src/drs_launch/launch/component/drs_camera.launch.xml
@@ -26,10 +26,11 @@
         <arg name="accelerated_image_processor_param_path" value="$(var camera_param_dir)/accelerated_image_processor.param.yaml"/>
       </include>
 
-      <!-- <!-\- Launch TF static publisher for camera and LiDAR -\-> -->
-      <!-- <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py"> -->
-      <!--   <arg name="tf_file_path" value="$(var camera_param_dir)/lidar_to_camera.json"/> -->
-      <!-- </include> -->
+      <!-- Launch TF periodic publisher for camera and LiDAR -->
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var camera_param_dir)/camera$(var camera_id)_calibration_results.yaml"/>
+      </include>
+
       <!-- Execute readout setter -->
       <node pkg="c2_readout_delay_setter" exec="c2_readout_delay_setter" name="readout_setter_$(var camera_id)">
         <param from="$(var camera_param_dir)/readout_delay.param.yaml"/>
diff --git a/src/drs_launch/launch/component/lidar_camera_tf_publisher.launch.py b/src/drs_launch/launch/component/tf_publisher.launch.py
similarity index 64%
rename from src/drs_launch/launch/component/lidar_camera_tf_publisher.launch.py
rename to src/drs_launch/launch/component/tf_publisher.launch.py
index e68a02a..1c3e94c 100644
--- a/src/drs_launch/launch/component/lidar_camera_tf_publisher.launch.py
+++ b/src/drs_launch/launch/component/tf_publisher.launch.py
@@ -15,7 +15,8 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.actions import OpaqueFunction
+from launch.actions import OpaqueFunction, GroupAction, PushLaunchConfigurations, PopLaunchConfigurations
+from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 import json
@@ -23,11 +24,15 @@
 import re
 from pathlib import Path
 import transforms3d
+from distutils.util import strtobool
 
-def parse_yaml(contents: dict):
+def parse_yaml(contents: dict, target_frame: str = None):
     get_first_key = lambda d: list(d.keys())[0]
     frame_id = get_first_key(contents)
-    child_frame_id = get_first_key(contents[frame_id])
+    if target_frame == "":
+        child_frame_id = get_first_key(contents[frame_id])
+    else:
+        child_frame_id = target_frame
 
     x = contents[frame_id][child_frame_id]['x']
     y = contents[frame_id][child_frame_id]['y']
@@ -53,6 +58,15 @@ def launch_setup(context, *args, **kwargs):
     """Return Launch Configuration for the TF between the lidar and the camera."""
     tf_file = LaunchConfiguration('tf_file_path').perform(context)
     file_format = Path(tf_file).suffix.lstrip('.')
+    target_frame = LaunchConfiguration('target_frame').perform(context)
+    publish_camera_optical_link = LaunchConfiguration('publish_camera_optical_link')
+    interval_sec = LaunchConfiguration('broadcast_interval_sec').perform(context)
+
+    launch_config_to_bool = lambda conf: bool(strtobool(conf.perform(context)))
+
+    print('')
+    print(f'tf_file: {tf_file}\n'
+          f'target_frame: {target_frame}')
 
     '''
     Here assumes json/yaml format generated by CalibrationTools
@@ -63,18 +77,22 @@ def launch_setup(context, *args, **kwargs):
     elif file_format == 'yaml' or file_format == 'yml':
         with open(tf_file, 'r') as f:
             yaml_contents = yaml.safe_load(f)
-            tf_data = parse_yaml(yaml_contents)
+            tf_data = parse_yaml(yaml_contents, target_frame)
             print(f'Parsed yaml contents: {tf_data}')
     else:
         print(f'Invalid TF file ({tf_file}) was specified.')
 
-    camera_id = re.findall(r'\d+', str(tf_data['child_frame_id']))[0]
+    if launch_config_to_bool(publish_camera_optical_link):
+        camera_id = re.findall(r'\d+', str(tf_data['child_frame_id']))[0]
+    else:
+        camera_id = ''
 
     return [
         Node(
-            name=f'lidar_camera_tf_publisher{camera_id}',
-            package='tf2_ros',
-            executable='static_transform_publisher',
+            # name=f'lidar_camera_tf_publisher{camera_id}',
+            # periodic_transform_publisher assigns unique node name with random postfix
+            package='periodic_transform_publisher',
+            executable='periodic_transform_publisher',
             arguments=[
                 '--x', str(tf_data['transform']['translation']['x']),
                 '--y', str(tf_data['transform']['translation']['y']),
@@ -85,11 +103,14 @@ def launch_setup(context, *args, **kwargs):
                 '--qw', str(tf_data['transform']['rotation']['w']),
                 '--frame-id', str(tf_data['header']['frame_id']),
                 '--child-frame-id', str(tf_data['child_frame_id'])
-            ]),
+            ],
+            parameters=[{
+                'interval_sec': float(interval_sec),
+            }]),
         Node(
-            name=f'camera_optical_link_publisher{camera_id}',
-            package='tf2_ros',
-            executable='static_transform_publisher',
+            name=f'periodic_camera_optical_link_publisher{camera_id}',
+            package='periodic_transform_publisher',
+            executable='periodic_transform_publisher',
             arguments=[
                 '--x', '0.0',
                 '--y', '0.0',
@@ -101,13 +122,20 @@ def launch_setup(context, *args, **kwargs):
                 '--frame-id', str(tf_data['child_frame_id']),
                 '--child-frame-id', str(tf_data['child_frame_id'].replace('camera_link',
                                                                           'camera_optical_link'))
-            ])
+            ],
+            parameters=[{
+                'interval_sec': float(interval_sec),
+            }],
+            condition=IfCondition(publish_camera_optical_link))
     ]
 
 def generate_launch_description():
     return LaunchDescription(
         [
             DeclareLaunchArgument("tf_file_path"),
-            OpaqueFunction(function=launch_setup),
+            DeclareLaunchArgument("target_frame", default_value=''),
+            DeclareLaunchArgument("publish_camera_optical_link", default_value='True'),
+            DeclareLaunchArgument("broadcast_interval_sec", default_value='1.0'),
+            OpaqueFunction(function=launch_setup)
         ]
     )
diff --git a/src/drs_launch/launch/drs_ecu0.launch.xml b/src/drs_launch/launch/drs_ecu0.launch.xml
index 1656a92..3ebfcd0 100644
--- a/src/drs_launch/launch/drs_ecu0.launch.xml
+++ b/src/drs_launch/launch/drs_ecu0.launch.xml
@@ -68,6 +68,30 @@
   <!--   <arg name="launch_driver" value="$(var live_sensor)"/> -->
   <!-- </include> -->
 
+  <!-- TF for each LiDARs and base_link to drs_base_link -->
+  <group if="$(var live_sensor)">
+    <group> <!-- guard by group not to polute argument dict by multiple calling-->
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_front"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_right"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/base_link_to_drs_base_link.yaml"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+  </group>
+
   <!-- topic monitor -->
   <group scoped="True">
     <!-- To prevent topic monitor communicate over network, set LOCALHOST_ONLY=1 -->
diff --git a/src/drs_launch/launch/drs_ecu1.launch.xml b/src/drs_launch/launch/drs_ecu1.launch.xml
index adc1b61..ac40958 100644
--- a/src/drs_launch/launch/drs_ecu1.launch.xml
+++ b/src/drs_launch/launch/drs_ecu1.launch.xml
@@ -63,6 +63,24 @@
     <arg name="launch_driver" value="$(var live_sensor)"/>
   </include>
 
+  <!-- TF for each LiDARs -->
+  <group if="$(var live_sensor)">
+    <group> <!-- guard by group not to polute argument dict by multiple calling-->
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_rear"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_left"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+  </group>
+
   <!-- topic monitor -->
   <group scoped="True">
     <!-- To prevent topic monitor communicate over network, set LOCALHOST_ONLY=1 -->
diff --git a/src/drs_launch/launch/drs_offline.launch.xml b/src/drs_launch/launch/drs_offline.launch.xml
index 8791e7b..62caa01 100644
--- a/src/drs_launch/launch/drs_offline.launch.xml
+++ b/src/drs_launch/launch/drs_offline.launch.xml
@@ -3,6 +3,7 @@
   <arg name="vehicle_id" default="default"/>
   <arg name="live_sensor" default="False"
        description="If true, sensor drivers will be executed (for actual recording operation)"/>
+  <arg name="publish_tf" default="True"/>
 
   <let name="param_root_dir" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)"/>
 
@@ -105,40 +106,81 @@
     <arg name="use_pointcloud_container" value="false"/>
   </include>
 
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
-          args="--frame-id base_link --child-frame-id drs_base_link --x 0.6895 --y 0.0 --z 1.971 --yaw 0.000 --pitch -0.020, --roll 0.000"/>
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
-        args="--frame-id drs_base_link --child-frame-id lidar_front --x 0.4734 --y 0.05 --z -0.0702 --yaw -1.5708 --pitch 0.000, --roll 0.005"/>
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
-        args="--frame-id drs_base_link --child-frame-id lidar_left --x 0.100 --y 0.560 --z -0.0832 --yaw 1.5708 --pitch 0.000, --roll 0.000"/>
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
-        args="--frame-id drs_base_link --child-frame-id lidar_right --x 0.100 --y -0.555 --z -0.0502 --yaw -1.583 --pitch 0.014, --roll 0.005"/>
-  <node pkg="tf2_ros" exec="static_transform_publisher" name="extrinsic_publisher"
-        args="--frame-id drs_base_link --child-frame-id lidar_rear --x -0.2494 --y 0.0 --z 0.0873 --yaw 3.142 --pitch 0.000, --roll 0.000"/>
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera0/camera0_calibration_results.yaml"/>
-  </include>  
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera1/camera1_calibration_results.yaml"/>
-  </include>  
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera2/camera2_calibration_results.yaml"/>
-  </include>  
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera3/camera3_calibration_results.yaml"/>
-  </include>  
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera4/camera4_calibration_results.yaml"/>
-  </include>  
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera5/camera5_calibration_results.yaml"/>
-  </include>  
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera6/camera6_calibration_results.yaml"/>
-  </include>  
-  <include file="$(find-pkg-share drs_launch)/launch/component/lidar_camera_tf_publisher.launch.py">
-    <arg name="tf_file_path" value="$(var param_root_dir)/camera7/camera7_calibration_results.yaml"/>
-  </include>  
-
+  <group if="$(var publish_tf)">
+    <group> <!-- guard by group not to polute argument dict by multiple calling-->
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/base_link_to_drs_base_link.yaml"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_front"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_left"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_right"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/drs_base_link_to_lidars.yaml"/>
+        <arg name="target_frame" value="lidar_rear"/>
+        <arg name="publish_camera_optical_link" value="False"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera0/camera0_calibration_results.yaml"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera1/camera1_calibration_results.yaml"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera2/camera2_calibration_results.yaml"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera3/camera3_calibration_results.yaml"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera4/camera4_calibration_results.yaml"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera5/camera5_calibration_results.yaml"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera6/camera6_calibration_results.yaml"/>
+      </include>
+    </group>
+    <group>
+      <include file="$(find-pkg-share drs_launch)/launch/component/tf_publisher.launch.py">
+        <arg name="tf_file_path" value="$(var param_root_dir)/camera7/camera7_calibration_results.yaml"/>
+      </include>
+    </group>
+  </group>
 
 </launch>
diff --git a/src/drs_launch/package.xml b/src/drs_launch/package.xml
index ae825a0..3510204 100644
--- a/src/drs_launch/package.xml
+++ b/src/drs_launch/package.xml
@@ -21,6 +21,7 @@
   <exec_depend>accelerated_image_processor</exec_depend>
   <exec_depend>c2_readout_delay_setter</exec_depend>
   <exec_depend>ros_topic_monitor</exec_depend>
+  <exec_depend>periodic_transform_publisher</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/src/individual_params/config/default/base_link_to_drs_base_link.yaml b/src/individual_params/config/default/base_link_to_drs_base_link.yaml
new file mode 100644
index 0000000..3df0911
--- /dev/null
+++ b/src/individual_params/config/default/base_link_to_drs_base_link.yaml
@@ -0,0 +1,9 @@
+base_link:
+  drs_base_link:
+    x: 0.6895
+    y: 0.0
+    z: 1.971
+    roll: 0.000
+    pitch: -0.020
+    yaw: 0.000
+
diff --git a/src/individual_params/config/default/drs_base_link_to_lidars.yaml b/src/individual_params/config/default/drs_base_link_to_lidars.yaml
new file mode 100644
index 0000000..1bbb651
--- /dev/null
+++ b/src/individual_params/config/default/drs_base_link_to_lidars.yaml
@@ -0,0 +1,30 @@
+drs_base_link:
+  lidar_left:
+    x: 0.100
+    y: 0.560
+    z: -0.0832
+    roll: 0.000
+    pitch: 0.000
+    yaw: 1.5708
+  lidar_right:
+    x: 0.100
+    y: -0.555
+    z: -0.0502
+    roll: 0.005
+    pitch: 0.014
+    yaw: -1.583
+  lidar_rear:
+    x: -0.2494
+    y: 0.0
+    z: 0.0873
+    roll: 0.000
+    pitch: 0.000
+    yaw: 3.142
+  lidar_front:
+    x: 0.4734
+    y: 0.05
+    z: -0.0702
+    roll: 0.005
+    pitch: 0.000
+    yaw: -1.5708
+