Merge pull request #5 from embasm/feature/tf_for_depth_image_projector

Use tf at depth_image_projector

Author: mohammedari

opengl_ros/CMakeLists.txt

@@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   opengl_ros_lib
-  realsense2_camera
+  tf
 )
 
 ## System dependencies are found with CMake's conventions

opengl_ros/launch/depth_image_projection.launch

@@ -41,6 +41,10 @@
 
   <node pkg="opengl_ros" type="depth_image_projector"
     name="depth_image_projector" output="screen">
+    <param name="color_frame_id"            value="d435_color_optical"/>
+    <param name="depth_frame_id"            value="d435_depth_optical"/>
+    <param name="tf_wait_duration"          value="0.1"/>
+
     <param name="colorWidth"                value="$(arg depth_width)"/>
     <param name="colorHeight"               value="$(arg depth_height)"/>
     <param name="depthWidth"                value="$(arg depth_width)"/>

opengl_ros/package.xml

@@ -45,7 +45,9 @@
   <build_depend>image_transport</build_depend>
   <build_depend>cv_bridge</build_depend>
   <build_depend>opengl_ros_lib</build_depend> 
+  <build_depend>tf</build_depend> 
   <run_depend>opengl_ros_lib</run_depend> 
+  <run_depend>tf</run_depend> 
 
 
   <!-- The export tag contains other, unspecified, tags -->

opengl_ros/src/depth_image_projector_nodecore.cpp

@@ -13,8 +13,10 @@ DepthImageProjectorNode::DepthImageProjectorNode(const ros::NodeHandle& nh, cons
     colorSubscriber_ = it_.subscribeCamera("color_in" , 1, &DepthImageProjectorNode::colorCallback, this);
     depthSubscriber_ = it_.subscribeCamera("depth_in" , 1, &DepthImageProjectorNode::depthCallback, this);
 
-    //TODO change to use TF and remove dependency on realsense2_camera package
-    depthToColorSubscriber_ = nh_.subscribe<realsense2_camera::Extrinsics>("depth_to_color" , 1, &DepthImageProjectorNode::depthToColorCallback, this);
+    // frame ids for tf which associates color frame with depth frame
+    nh_.param<std::string>("color_frame_id", color_frame_id_, "d435_color_optical");
+    nh_.param<std::string>("depth_frame_id", depth_frame_id_, "d435_depth_optical");
+    nh_.param<double>("tf_wait_duration", tf_wait_duration_, 0.1);
 
     int colorWidth, colorHeight;
     nh_.param<int>("colorWidth" , colorWidth , 640);
@@ -92,7 +94,9 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
         return;
     }
 
-    if (!depthToColorArrived_)
+    // Update & check whether depthToColor is valid.
+    bool is_conversion_valid = updateDepthToColor();
+    if (!is_conversion_valid)
     {
         ROS_WARN_STREAM("extrinsics parameter not arrivied yet");
         return;
@@ -122,21 +126,42 @@ void DepthImageProjectorNode::depthCallback(const sensor_msgs::Image::ConstPtr&
     imagePublisher_.publish(outImage.toImageMsg());
 }
 
-void DepthImageProjectorNode::depthToColorCallback(const realsense2_camera::Extrinsics::ConstPtr& depthToColorMsg)
+bool DepthImageProjectorNode::updateDepthToColor()
 {
-    auto& r = depthToColorMsg->rotation;
-    auto& t = depthToColorMsg->translation;
-    latestDepthToColor_ = {
-        static_cast<float>(r[0]), static_cast<float>(r[1]), static_cast<float>(r[2]), 0, 
-        static_cast<float>(r[3]), static_cast<float>(r[4]), static_cast<float>(r[5]), 0, 
-        static_cast<float>(r[6]), static_cast<float>(r[7]), static_cast<float>(r[8]), 0, 
-        static_cast<float>(t[0]), static_cast<float>(t[1]), static_cast<float>(t[2]), 1,
-    }; //column major order
-
-    depthToColorArrived_ = true;
+    // Calculation is performed only once since depthToColor does not change.
+    if (depthToColorArrived_)
+    {
+        return true;
+    }
+
+    try
+    {
+        if (tfListener_.waitForTransform(color_frame_id_, depth_frame_id_, ros::Time(0), ros::Duration(tf_wait_duration_)))
+        {
+            tf::StampedTransform transform;
+            tfListener_.lookupTransform(color_frame_id_, depth_frame_id_, ros::Time(0), transform);
+
+            const tf::Matrix3x3& R = transform.getBasis();
+            const tf::Vector3& t =  transform.getOrigin();
+
+            latestDepthToColor_ = {
+                static_cast<float>(R[0][0]), static_cast<float>(R[1][0]), static_cast<float>(R[2][0]), 0.0,
+                static_cast<float>(R[0][1]), static_cast<float>(R[1][1]), static_cast<float>(R[2][1]), 0.0,
+                static_cast<float>(R[0][2]), static_cast<float>(R[1][2]), static_cast<float>(R[2][2]), 0.0,
+                static_cast<float>(t[0]),    static_cast<float>(t[1]),    static_cast<float>(t[2]),    1.0,
+            }; //column major order
+            depthToColorArrived_ = true;
+        }
+    }
+    catch (tf::TransformException& ex)
+    {
+        ROS_WARN("Transform Exception in updateDepthToColor(). %s", ex.what());
+    }
+
+    return depthToColorArrived_;
 }
 
 void DepthImageProjectorNode::run()
 {
     ros::spin();
-}
+}

opengl_ros/src/depth_image_projector_nodecore.h

@@ -6,9 +6,9 @@
 
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
-#include <realsense2_camera/Extrinsics.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
+#include <tf/transform_listener.h>
 
 #include "depth_image_projector.h"
 
@@ -20,6 +20,12 @@ class DepthImageProjectorNode
     ros::NodeHandle nh_;
     image_transport::ImageTransport it_;
 
+    // tf
+    std::string color_frame_id_;
+    std::string depth_frame_id_;
+    tf::TransformListener tfListener_;
+    double tf_wait_duration_;
+
     //Publishers and Subscribers
     image_transport::Publisher imagePublisher_;
     image_transport::CameraSubscriber colorSubscriber_;
@@ -36,7 +42,7 @@ class DepthImageProjectorNode
     void depthCallback(const sensor_msgs::Image::ConstPtr& imageMsg, const sensor_msgs::CameraInfoConstPtr & cameraInfoMsg);
     bool depthToColorArrived_ = false;
     std::array<float, 16> latestDepthToColor_;
-    void depthToColorCallback(const realsense2_camera::Extrinsics::ConstPtr& depthToColorMsg);
+    bool updateDepthToColor();
 
 public:
     DepthImageProjectorNode(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private);
@@ -45,4 +51,4 @@ class DepthImageProjectorNode
 
 } //opengl_ros
 
-#endif
+#endif