From 058958e38cc394f273f53a157c5504dc4551b6f1 Mon Sep 17 00:00:00 2001
From: Stefan Kohlbrecher <kohlbrecher@sim.tu-darmstadt.de>
Date: Tue, 30 Aug 2016 11:31:54 +0200
Subject: [PATCH] Adds lua parameter for setting transform wait time (#33)

---
 cartographer_ros/configuration_files/backpack_2d.lua | 1 +
 cartographer_ros/configuration_files/backpack_3d.lua | 1 +
 cartographer_ros/configuration_files/turtlebot.lua   | 1 +
 cartographer_ros/src/cartographer_node_main.cc       | 6 ++++--
 4 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua
index adeb670..5d806df 100644
--- a/cartographer_ros/configuration_files/backpack_2d.lua
+++ b/cartographer_ros/configuration_files/backpack_2d.lua
@@ -26,6 +26,7 @@ options = {
   laser_min_range = 0.,
   laser_max_range = 30.,
   laser_missing_echo_ray_length = 5.,
+  lookup_transform_timeout = 0.01,
   use_multi_echo_laser_scan_2d = true
 }
 
diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua
index dc1074e..a2c9966 100644
--- a/cartographer_ros/configuration_files/backpack_3d.lua
+++ b/cartographer_ros/configuration_files/backpack_3d.lua
@@ -26,6 +26,7 @@ options = {
   laser_min_range = 0.,
   laser_max_range = 30.,
   laser_missing_echo_ray_length = 5.,
+  lookup_transform_timeout = 0.01,
   num_lasers_3d = 2
 }
 
diff --git a/cartographer_ros/configuration_files/turtlebot.lua b/cartographer_ros/configuration_files/turtlebot.lua
index 2b75c89..6298503 100644
--- a/cartographer_ros/configuration_files/turtlebot.lua
+++ b/cartographer_ros/configuration_files/turtlebot.lua
@@ -26,6 +26,7 @@ options = {
   laser_min_range = 0.,
   laser_max_range = 30.,
   laser_missing_echo_ray_length = 5.,
+  lookup_transform_timeout = 0.01,
   use_laser_scan_2d = true
 }
 
diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc
index ce80bbe..8e76507 100644
--- a/cartographer_ros/src/cartographer_node_main.cc
+++ b/cartographer_ros/src/cartographer_node_main.cc
@@ -94,7 +94,6 @@ constexpr int64 kTrajectoryId = 0;
 constexpr int kSubscriberQueueSize = 150;
 constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll;  // 300 milliseconds
 constexpr int kPosePublishPeriodInUts = 5 * 10000ll;      // 5 milliseconds
-constexpr double kMaxTransformDelaySeconds = 0.01;
 constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.;
 
 // Unique default topic names. Expected to be remapped as needed.
@@ -248,6 +247,7 @@ class Node {
   double laser_min_range_;
   double laser_max_range_;
   double laser_missing_echo_ray_length_;
+  double lookup_transform_timeout_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_;
@@ -282,7 +282,7 @@ Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
                                                const string& frame_id) {
   return ToRigid3d(
       tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time),
-                                 ros::Duration(kMaxTransformDelaySeconds)));
+                                 ros::Duration(lookup_transform_timeout_)));
 }
 
 void Node::OdometryMessageCallback(const nav_msgs::Odometry::ConstPtr& msg) {
@@ -416,6 +416,8 @@ void Node::Initialize() {
   laser_max_range_ = lua_parameter_dictionary.GetDouble("laser_max_range");
   laser_missing_echo_ray_length_ =
       lua_parameter_dictionary.GetDouble("laser_missing_echo_ray_length");
+  lookup_transform_timeout_ =
+      lua_parameter_dictionary.GetDouble("lookup_transform_timeout");
 
   // Set of all topics we subscribe to. We use the non-remapped default names
   // which are unique.