From 08fd374e7fe8c844aaa5d70a4d88de3816aacd93 Mon Sep 17 00:00:00 2001 From: Joshua Curtis Date: Fri, 30 Nov 2018 14:58:32 -0700 Subject: [PATCH] Now generates point cloud rgb from the depth image. --- realsense2_camera/src/base_realsense_node.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index fd8c3e7d9c..e73d7f9ae1 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -691,7 +691,7 @@ void BaseRealSenseNode::setupStreams() _image[DEPTH].data = (uint8_t*)depth_frame.get_data(); } - if (_enable_filter && is_frame_arrived.at(COLOR)) + if (_enable_filter && is_frame_arrived.at(INFRARGB)) { publishFrame(f, t, sip, @@ -1171,7 +1171,7 @@ void BaseRealSenseNode::publishRgbToDepthPCTopic(const ros::Time& t, const std:: { try { - if (!is_frame_arrived.at(COLOR) || !is_frame_arrived.at(DEPTH)) + if (!is_frame_arrived.at(INFRARGB) || !is_frame_arrived.at(DEPTH)) { ROS_DEBUG("Skipping publish PC topic! Color or Depth frame didn't arrive."); return; @@ -1214,11 +1214,13 @@ void BaseRealSenseNode::publishRgbToDepthPCTopic(const ros::Time& t, const std:: void BaseRealSenseNode::publishITRTopic(sensor_msgs::PointCloud2& msg_pointcloud, bool colorized_pointcloud) { - auto& depth2color_extrinsics = _depth_to_other_extrinsics[COLOR]; - auto color_intrinsics = _stream_intrinsics[COLOR]; + auto infrargb_intrinsics = _stream_intrinsics[INFRARGB]; auto depth_intrinsics = _stream_intrinsics[DEPTH]; auto image_depth16 = reinterpret_cast(_image[DEPTH].data); - unsigned char* color_data = _image[COLOR].data; + auto width_ratio = static_cast(infrargb_intrinsics.width) / static_cast(depth_intrinsics.width); + auto height_ratio = static_cast(infrargb_intrinsics.height) / static_cast(depth_intrinsics.height); + + unsigned char* color_data = _image[INFRARGB].data; const float MIN_DEPTH_THRESHOLD = 0.f; const float MAX_DEPTH_THRESHOLD = 5.f; float depth_point[3], scaled_depth; @@ -1240,7 +1242,6 @@ void BaseRealSenseNode::publishITRTopic(sensor_msgs::PointCloud2& msg_pointcloud scaled_depth = static_cast(*image_depth16) * _depth_scale_meters; float depth_pixel[2] = {static_cast(x), static_cast(y)}; rs2_deproject_pixel_to_point(depth_point, &depth_intrinsics, depth_pixel, scaled_depth); - if (depth_point[2] <= MIN_DEPTH_THRESHOLD || depth_point[2] > MAX_DEPTH_THRESHOLD) { depth_point[0] = MIN_DEPTH_THRESHOLD;