Transform the laserscan data coordinate system

CMakeLists.txt

@@ -10,6 +10,8 @@ find_package(image_geometry REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 include_directories(include)
 
@@ -20,6 +22,8 @@ ament_target_dependencies(DepthImageToLaserScan
   "image_geometry"
   "OpenCV"
   "sensor_msgs"
+  tf2
+  tf2_ros
 )
 
 add_library(DepthImageToLaserScanROS

cfg/Depth.cfg

@@ -12,6 +12,7 @@ gen.add("scan_height",          int_t,    0,                                "Hei
 gen.add("scan_time",            double_t, 0,                                "Time for the entire scan sweep.",                                  0.033,  0.0, 1.0)
 gen.add("range_min",            double_t, 0,                                "Minimum reported range (in meters).",                              0.45,   0.0, 10.0)
 gen.add("range_max",            double_t, 0,                                "Maximum reported range (in meters).",                              10.0,   0.0, 10.0)
-gen.add("output_frame_id",      str_t,    0,                                "Output frame_id for the laserscan.",   "camera_depth_frame")
+gen.add("output_frame",         str_t,    0,                                "Output frame_id for the laserscan.",   "laser")
+gen.add("depth_optical_frame",  str_t,    0,                                "Output frame_id for the camera_depth_optical_frame.",   "camera_depth_optical_frame")
 
-exit(gen.generate(PACKAGE, "depthimage_to_laserscan", "Depth"))
+exit(gen.generate(PACKAGE, "depthimage_to_laserscan", "Depth"))

include/depthimage_to_laserscan/DepthImageToLaserScan.h

@@ -46,6 +46,11 @@
 #include <opencv2/core/core.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 
+#include <tf2/LinearMath/Transform.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2/transform_datatypes.h>
+
 namespace depthimage_to_laserscan
 { 
   class DepthImageToLaserScan
@@ -115,6 +120,7 @@ namespace depthimage_to_laserscan
      * 
      */
     void set_output_frame(const std::string output_frame_id);
+    void set_depth_optical_frame(const std::string depth_optical_frame);
 
   private:
     /**
@@ -174,39 +180,49 @@ namespace depthimage_to_laserscan
                  const sensor_msgs::msg::LaserScan::SharedPtr& scan_msg, const int& scan_height) const{
       // Use correct principal point from calibration
       float center_x = cam_model.cx();
+      float center_y = cam_model.cy();
 
       // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
       double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );
       float constant_x = unit_scaling / cam_model.fx();
-
+      float constant_y = unit_scaling / cam_model.fy();
+
       const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
       int row_step = depth_msg->step / sizeof(T);
 
       int offset = static_cast<int>(cam_model.cy() - scan_height / 2);
       depth_row += offset*row_step; // Offset to center of image
       for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){
-		for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row
-		{	
-		  T depth = depth_row[u];
-
-		  double r = depth; // Assign to pass through NaNs and Infs
-		  double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
-		  int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
-
-		  if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
-		    // Calculate in XYZ
-		    double x = (u - center_x) * depth * constant_x;
-		    double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
-
-		    // Calculate actual distance
-		    r = sqrt(pow(x, 2.0) + pow(z, 2.0));
-		  }
-
-	  // Determine if this point should be used.
-	  if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
-	    scan_msg->ranges[index] = r;
-	  }
-	}
+        for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row
+        {	
+          T depth = depth_row[u];
+
+          if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
+            // Calculate in XYZ
+            double dx = (center_x - u) * depth * constant_x;
+            double dy = (center_y - v) * depth * constant_y;
+            double dz = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
+
+            double X, Y, Z;
+            /* ROS TF 变换 */
+            tf2::Transform obj2depthDevice_tf(tf2::Quaternion(0, 0, 0, 1), tf2::Vector3(-dx, -dy, dz));
+            tf2::Transform XYZ2laser_tf = depthDevice2output_tf_ * obj2depthDevice_tf;
+            X = XYZ2laser_tf.getOrigin().x();
+            Y = XYZ2laser_tf.getOrigin().y();
+            Z = XYZ2laser_tf.getOrigin().z();
+
+            double r = depth; // Assign to pass through NaNs and Infs
+            int index = (atan2(Y, X) - scan_msg->angle_min) / scan_msg->angle_increment + 0.5; //0.5 -> rounding 
+
+            // Calculate actual distance
+            r = sqrt(pow(X, 2.0) + pow(Y, 2.0));
+
+            // Determine if this point should be used.
+            if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
+              scan_msg->ranges[index] = r;
+            }
+          }
+	      }
       }
     }
 
@@ -217,6 +233,10 @@ namespace depthimage_to_laserscan
     float range_max_; ///< Stores the current maximum range to use.
     int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area.
     std::string output_frame_id_; ///< Output frame_id for each laserscan.  This is likely NOT the camera's frame_id.
+    std::string depth_optical_frame_;
+public:
+    tf2::Transform depthDevice2output_tf_;
+
   };
 
 

package.xml

@@ -17,6 +17,8 @@
   <depend>libopencv-dev</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
 

src/DepthImageToLaserScan.cpp

@@ -170,3 +170,7 @@ void DepthImageToLaserScan::set_scan_height(const int scan_height){
 void DepthImageToLaserScan::set_output_frame(const std::string output_frame_id){
   output_frame_id_ = output_frame_id;
 }
+
+void DepthImageToLaserScan::set_depth_optical_frame(const std::string depth_optical_frame){
+  depth_optical_frame_ = depth_optical_frame;
+}

src/DepthImageToLaserScanROS.cpp

@@ -65,8 +65,8 @@ DepthImageToLaserScanROS::DepthImageToLaserScanROS(rclcpp::Node::SharedPtr & nod
   node_->get_parameter("scan_time", scan_time);
   dtl_.set_scan_time(scan_time);
 
-  float range_min = 0.45;
-  float range_max = 10.0;
+  float range_min = 0.2;
+  float range_max = 4.0;
   node_->get_parameter("range_min", range_min);
   node_->get_parameter("range_max", range_max);
   dtl_.set_range_limits(range_min, range_max);
@@ -75,9 +75,55 @@ DepthImageToLaserScanROS::DepthImageToLaserScanROS(rclcpp::Node::SharedPtr & nod
   node_->get_parameter("scan_height", scan_height);
   dtl_.set_scan_height(scan_height);
 
-  std::string output_frame = "camera_depth_frame";
+  std::string output_frame = "base_link";
   node_->get_parameter("output_frame", output_frame);
   dtl_.set_output_frame(output_frame);
+
+  std::string depth_optical_frame = "camera_depth_optical_frame";
+  node_->get_parameter("depth_optical_frame", depth_optical_frame);
+  dtl_.set_depth_optical_frame(depth_optical_frame);
+
+
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
+  tf_buffer_->setUsingDedicatedThread(true);
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  RCLCPP_INFO(node_->get_logger(), "Transform from  depth_optical_frame to output frame!");
+  rclcpp::Time transform_time = node_->now();
+  std::string tf_error;
+  while (rclcpp::ok() && !tf_buffer_->canTransform(depth_optical_frame, output_frame, tf2_ros::fromMsg(transform_time),
+    tf2::durationFromSec(3.0), &tf_error)) //tf2::TimePointZero
+  {
+    RCLCPP_INFO(node_->get_logger(), "Timed out waiting for transform from %s to %s"
+      " to become available, tf error: %s",
+      depth_optical_frame.c_str(), output_frame.c_str(), tf_error.c_str());
+    tf_error.clear();
+  }
+
+  geometry_msgs::msg::TransformStamped tf_depthOpticalFrame2outputFrame;
+  try {
+    tf_depthOpticalFrame2outputFrame = tf_buffer_->lookupTransform(output_frame, depth_optical_frame, tf2::TimePointZero);
+    RCLCPP_INFO(node_->get_logger(), "transform from %s to %s is : x = %.4f; y = %.4f; z = %.4f;", 
+                depth_optical_frame.c_str(), 
+                output_frame.c_str(),
+                tf_depthOpticalFrame2outputFrame.transform.translation.x,
+                tf_depthOpticalFrame2outputFrame.transform.translation.y,
+                tf_depthOpticalFrame2outputFrame.transform.translation.z);
+    dtl_.depthDevice2output_tf_.setOrigin(tf2::Vector3(
+        tf_depthOpticalFrame2outputFrame.transform.translation.x,
+        tf_depthOpticalFrame2outputFrame.transform.translation.y,
+        tf_depthOpticalFrame2outputFrame.transform.translation.z)); 
+
+    dtl_.depthDevice2output_tf_.setRotation(tf2::Quaternion(
+        tf_depthOpticalFrame2outputFrame.transform.rotation.x,
+        tf_depthOpticalFrame2outputFrame.transform.rotation.y,
+        tf_depthOpticalFrame2outputFrame.transform.rotation.z,
+        tf_depthOpticalFrame2outputFrame.transform.rotation.w));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR(node_->get_logger(), ex.what());
+  }
 }
 
 DepthImageToLaserScanROS::~DepthImageToLaserScanROS()