updating tf_relay to have a dynamically-reconfigurable delay

Author: jbohren

CMakeLists.txt

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.6.3)
 
 project(lcsr_tf_tools)
 
-find_package(catkin REQUIRED tf geometry_msgs interactive_markers visualization_msgs message_generation)
+find_package(catkin REQUIRED tf geometry_msgs interactive_markers visualization_msgs message_generation std_msgs)
 
 add_message_files(
   FILES StaticTransform.msg)

include/lcsr_tf_tools/tf_relay.h

@@ -4,6 +4,7 @@
 #include <ros/ros.h>
 #include <ros/time.h>
 #include <tf/tfMessage.h>
+#include <std_msgs/Float64.h>
 #include <boost/unordered_set.hpp>
 #include <queue>
 
@@ -32,6 +33,7 @@ class TFRelay {
 
 protected:
 
+  void set_delay_cb(const std_msgs::Float64 &msg);
   void cb(const tf::tfMessageConstPtr &msg);
 
   ros::Duration delay_;
@@ -44,7 +46,8 @@ class TFRelay {
       buffered_transforms_;
   boost::unordered_set<std::string> frame_ids_;
   boost::unordered_set<std::string> filtered_frame_ids_;
-  ros::Subscriber sub_;
+  ros::Subscriber tf_sub_;
+  ros::Subscriber set_delay_sub_;
   ros::Publisher pub_;
 };
 

package.xml

@@ -22,6 +22,7 @@
   <run_depend>geometry_msgs</run_depend>
   <run_depend>interactive_markers</run_depend>
   <run_depend>visualization_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
 </package>
 
 

src/tf_relay.cpp

@@ -44,7 +44,8 @@ TFRelay::TFRelay(ros::NodeHandle nh) :
   if(use_udp) {
     hints = hints.udp();
   }
-  sub_ = nh.subscribe("tf_in", 300, &TFRelay::cb, this, hints);
+  tf_sub_ = nh.subscribe("tf_in", 300, &TFRelay::cb, this, hints);
+  set_delay_sub_ = nhp.subscribe("set_delay", 1, &TFRelay::set_delay_cb, this);
 }
 
 void TFRelay::sleep()
@@ -63,6 +64,12 @@ void TFRelay::broadcast()
   msg_.transforms.clear();
 }
 
+void TFRelay::set_delay_cb(const std_msgs::Float64 &msg)
+{
+  delay_ = ros::Duration(msg.data);
+}
+
+
 void TFRelay::cb(const tf::tfMessageConstPtr &msg)
 {
   // Filter the messages based on child_frame_id