Add dynamic transform support again.

#169
#660

realsense2_camera/include/base_realsense_node.h

@@ -20,6 +20,7 @@
 #include <queue>
 #include <mutex>
 #include <atomic>
+#include <thread>
 
 namespace realsense2_camera
 {
@@ -218,6 +219,7 @@ namespace realsense2_camera
         void updateStreamCalibData(const rs2::video_stream_profile& video_profile);
         void SetBaseStream();
         void publishStaticTransforms();
+        void publishDynamicTransforms();
         void publishIntrinsics();
         void runFirstFrameInitialization(rs2_stream stream_type);
         void publishPointCloud(rs2::points f, const ros::Time& t, const rs2::frameset& frameset);
@@ -274,7 +276,12 @@ namespace realsense2_camera
         std::map<stream_index_pair, int> _fps;
         std::map<stream_index_pair, bool> _enable;
         std::map<rs2_stream, std::string> _stream_name;
+        bool _publish_tf;
+        double _tf_publish_rate;
         tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster;
+        tf2_ros::TransformBroadcaster _dynamic_tf_broadcaster;
+        std::vector<geometry_msgs::TransformStamped> _static_tf_msgs;
+        std::shared_ptr<std::thread> _tf_t;
 
         std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics> _image_publishers;
         std::map<stream_index_pair, ros::Publisher> _imu_publishers;

realsense2_camera/include/constants.h

@@ -41,6 +41,9 @@ namespace realsense2_camera
     const bool ALLOW_NO_TEXTURE_POINTS = false;
     const bool SYNC_FRAMES    = false;
 
+    const bool PUBLISH_TF        = true;
+    const double TF_PUBLISH_RATE = 0; // Static transform
+
     const int IMAGE_WIDTH     = 640;
     const int IMAGE_HEIGHT    = 480;
     const int IMAGE_FPS       = 30;

realsense2_camera/launch/includes/nodelet.launch.xml

@@ -72,6 +72,9 @@
   <arg name="aligned_depth_to_fisheye1_frame_id" default="$(arg tf_prefix)_aligned_depth_to_fisheye1_frame"/>
   <arg name="aligned_depth_to_fisheye2_frame_id" default="$(arg tf_prefix)_aligned_depth_to_fisheye2_frame"/>
 
+  <arg name="publish_tf"               default="true"/>
+  <arg name="tf_publish_rate"          default="0"/> <!-- 0 - static transform -->
+
   <arg name="odom_frame_id"            default="$(arg tf_prefix)_odom_frame"/>
   <arg name="topic_odom_in"            default="$(arg tf_prefix)/odom_in"/>
   <arg name="calib_odom_file"          default=""/>
@@ -155,6 +158,9 @@
     <param name="aligned_depth_to_fisheye1_frame_id" type="str"  value="$(arg aligned_depth_to_fisheye1_frame_id)"/>
     <param name="aligned_depth_to_fisheye2_frame_id" type="str"  value="$(arg aligned_depth_to_fisheye2_frame_id)"/>
 
+    <param name="publish_tf"               type="bool"   value="$(arg publish_tf)"/>
+    <param name="tf_publish_rate"          type="double" value="$(arg tf_publish_rate)"/>
+
     <param name="odom_frame_id"            type="str"  value="$(arg odom_frame_id)"/>
     <param name="topic_odom_in"            type="str"  value="$(arg topic_odom_in)"/>
     <param name="calib_odom_file"          type="str"    value="$(arg calib_odom_file)"/>

realsense2_camera/launch/rs_camera.launch

@@ -39,6 +39,9 @@
   <arg name="enable_sync"               default="false"/>
   <arg name="align_depth"               default="false"/>
 
+  <arg name="publish_tf"                default="true"/>
+  <arg name="tf_publish_rate"           default="0"/>
+
   <arg name="filters"                   default=""/>
   <arg name="clip_distance"             default="-2"/>
   <arg name="linear_accel_cov"          default="0.01"/>
@@ -89,6 +92,9 @@
       <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
       <arg name="enable_accel"             value="$(arg enable_accel)"/>
 
+      <arg name="publish_tf"               value="$(arg publish_tf)"/>
+      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>
+
       <arg name="filters"                  value="$(arg filters)"/>
       <arg name="clip_distance"            value="$(arg clip_distance)"/>
       <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>

realsense2_camera/src/base_realsense_node.cpp

@@ -4,8 +4,6 @@
 #include <algorithm>
 #include <cctype>
 #include <mutex>
-#include <tf/transform_broadcaster.h>
-#include <thread>
 
 using namespace realsense2_camera;
 using namespace ddynamic_reconfigure;
@@ -128,6 +126,10 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle,
 
 BaseRealSenseNode::~BaseRealSenseNode()
 {
+    // Kill dynamic transform thread
+    if (_publish_tf && _tf_publish_rate > 0)
+        _tf_t->join();
+
     _is_running = false;
     _cv.notify_one();
     if (_monitoring_t && _monitoring_t->joinable())
@@ -191,6 +193,15 @@ void BaseRealSenseNode::publishTopics()
     SetBaseStream();
     registerAutoExposureROIOptions(_node_handle);
     publishStaticTransforms();
+    if (_publish_tf)
+    {
+        // Static transform for non-positive values
+        if (_tf_publish_rate > 0)
+            _tf_t = std::shared_ptr<std::thread>(new std::thread(boost::bind(&BaseRealSenseNode::publishDynamicTransforms, this)));
+        else
+            _static_tf_broadcaster.sendTransform(_static_tf_msgs);
+    }
+
     publishIntrinsics();
     startMonitoring();
     ROS_INFO_STREAM("RealSense Node Is Up!");
@@ -531,6 +542,9 @@ void BaseRealSenseNode::getParameters()
     _pnh.param("filters", _filters_str, DEFAULT_FILTERS);
     _pointcloud |= (_filters_str.find("pointcloud") != std::string::npos);
 
+    _pnh.param("publish_tf", _publish_tf, PUBLISH_TF);
+    _pnh.param("tf_publish_rate", _tf_publish_rate, TF_PUBLISH_RATE);
+
     _pnh.param("enable_sync", _sync_frames, SYNC_FRAMES);
     if (_pointcloud || _align_depth || _filters_str.size() > 0)
         _sync_frames = true;
@@ -1814,7 +1828,7 @@ void BaseRealSenseNode::publish_static_tf(const ros::Time& t,
     msg.transform.rotation.y = q.getY();
     msg.transform.rotation.z = q.getZ();
     msg.transform.rotation.w = q.getW();
-    _static_tf_broadcaster.sendTransform(msg);
+    _static_tf_msgs.push_back(msg);
 }
 
 void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile)
@@ -1880,13 +1894,17 @@ void BaseRealSenseNode::SetBaseStream()
 
 void BaseRealSenseNode::publishStaticTransforms()
 {
-    // Publish static transforms
     rs2::stream_profile base_profile = getAProfile(_base_stream);
-    for (std::pair<stream_index_pair, bool> ienable : _enable)
+
+    // Publish static transforms
+    if (_publish_tf)
     {
-        if (ienable.second)
+        for (std::pair<stream_index_pair, bool> ienable : _enable)
         {
-            calcAndPublishStaticTransform(ienable.first, base_profile);
+            if (ienable.second)
+            {
+                calcAndPublishStaticTransform(ienable.first, base_profile);
+            }
         }
     }
 
@@ -1930,6 +1948,26 @@ void BaseRealSenseNode::publishStaticTransforms()
 
 }
 
+void BaseRealSenseNode::publishDynamicTransforms()
+{
+    // Publish transforms for the cameras
+    ROS_WARN("Publishing dynamic camera transforms (/tf) at %.f Hz", _tf_publish_rate);
+
+    ros::Rate loop_rate(_tf_publish_rate);
+
+    while (ros::ok())
+    {
+        // Update the time stamp for publication
+        ros::Time t = ros::Time::now();
+        for(auto& msg : _static_tf_msgs)
+            msg.header.stamp = t;
+
+        _dynamic_tf_broadcaster.sendTransform(_static_tf_msgs);
+
+        loop_rate.sleep();
+    }
+}
+
 void BaseRealSenseNode::publishIntrinsics()
 {
     if (_enable[GYRO])