diff --git a/CMakeLists.txt b/CMakeLists.txt index 38dd59b..5ac2155 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/cfg/Depth.cfg b/cfg/Depth.cfg index 127c704..65eb244 100755 --- a/cfg/Depth.cfg +++ b/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")) \ No newline at end of file +exit(gen.generate(PACKAGE, "depthimage_to_laserscan", "Depth")) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.h b/include/depthimage_to_laserscan/DepthImageToLaserScan.h index d5f600c..c761a75 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.h +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.h @@ -46,6 +46,11 @@ #include #include +#include +#include +#include +#include + 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::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(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); int offset = static_cast(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::valid(depth)){ // Not NaN or Inf - // Calculate in XYZ - double x = (u - center_x) * depth * constant_x; - double z = depthimage_to_laserscan::DepthTraits::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::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::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_; + }; diff --git a/package.xml b/package.xml index 886152b..521558b 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,8 @@ libopencv-dev rclcpp sensor_msgs + tf2 + tf2_ros ament_cmake_gtest diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index dfeb332..77e245d 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/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; +} \ No newline at end of file diff --git a/src/DepthImageToLaserScanROS.cpp b/src/DepthImageToLaserScanROS.cpp index 40e3dc2..35cfedf 100644 --- a/src/DepthImageToLaserScanROS.cpp +++ b/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 tf_listener_; + std::shared_ptr tf_buffer_; + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_buffer_->setUsingDedicatedThread(true); + tf_listener_ = std::make_shared(*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()