diff --git a/README.md b/README.md index d7917581ec..8d71d5edf5 100644 --- a/README.md +++ b/README.md @@ -55,9 +55,7 @@ This will launch [RViz](http://wiki.ros.org/rviz) and display the depth pointclo ## Known Issues -* This ROS node does not currently provide RGB point cloud. * This ROS node does not currently provide any dynamic reconfigure support for camera properties/presets. -* This ROS node does not currently offer temporal syncronization of depth and color data. * This ROS node does not currently support [ROS Lunar Loggerhead](http://wiki.ros.org/lunar). * This ROS node does not currently work with [ROS 2](https://github.com/ros2/ros2/wiki). * This ROS node currently does not provide the unit-tests which ensure the proper operation of the camera. Future versions of the node will provide ROS compatible unit-tests. diff --git a/realsense_ros_camera/include/constants.h b/realsense_ros_camera/include/constants.h index b84d25fff3..e950a07f37 100644 --- a/realsense_ros_camera/include/constants.h +++ b/realsense_ros_camera/include/constants.h @@ -9,7 +9,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 0 -#define REALSENSE_ROS_PATCH_VERSION 0 +#define REALSENSE_ROS_PATCH_VERSION 1 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) @@ -18,6 +18,9 @@ namespace realsense_ros_camera { + const bool POINTCLOUD = false; + const bool SYNC_FRAMES = false; + const int DEPTH_WIDTH = 640; const int DEPTH_HEIGHT = 480; diff --git a/realsense_ros_camera/launch/demo_pointcloud.launch b/realsense_ros_camera/launch/demo_pointcloud.launch index 21b656afb2..00b7161173 100644 --- a/realsense_ros_camera/launch/demo_pointcloud.launch +++ b/realsense_ros_camera/launch/demo_pointcloud.launch @@ -2,11 +2,21 @@ - - - - - + + + + + + + + + + + + + + + diff --git a/realsense_ros_camera/launch/rs_camera.launch b/realsense_ros_camera/launch/rs_camera.launch index 394c94144d..adef737090 100644 --- a/realsense_ros_camera/launch/rs_camera.launch +++ b/realsense_ros_camera/launch/rs_camera.launch @@ -38,9 +38,15 @@ + + + + + + diff --git a/realsense_ros_camera/package.xml b/realsense_ros_camera/package.xml index 7944e27a73..cb715abf83 100644 --- a/realsense_ros_camera/package.xml +++ b/realsense_ros_camera/package.xml @@ -1,7 +1,7 @@ realsense_ros_camera - 1.0.0 + 2.0.1 The realsense_ros_camera package Intel RealSense Apache 2.0 diff --git a/realsense_ros_camera/rviz/pointcloud.rviz b/realsense_ros_camera/rviz/pointcloud.rviz index f136a01aa6..97eb4cb20b 100644 --- a/realsense_ros_camera/rviz/pointcloud.rviz +++ b/realsense_ros_camera/rviz/pointcloud.rviz @@ -55,8 +55,8 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz/PointCloud2 - Color: 85; 0; 255 - Color Transformer: FlatColor + Color: 255; 255; 255 + Color Transformer: RGB8 Decay Time: 0 Enabled: true Invert Rainbow: false @@ -68,9 +68,9 @@ Visualization Manager: Position Transformer: XYZ Queue Size: 10 Selectable: true - Size (Pixels): 1 + Size (Pixels): 3 Size (m): 0.00999999978 - Style: Points + Style: Flat Squares Topic: /camera/points Unreliable: false Use Fixed Frame: true @@ -100,24 +100,25 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 3.48406529 + Distance: 6.36999989 Enable Stereo Rendering: Stereo Eye Separation: 0.0599999987 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.697073579 - Y: -0.280089796 + X: 0 + Y: 0 Z: 0 - Focal Shape Fixed Size: true + Focal Shape Fixed Size: false Focal Shape Size: 0.0500000007 + Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 0.659797728 + Pitch: 0.085398294 Target Frame: Value: ThirdPersonFollower (rviz) - Yaw: 3.15039515 + Yaw: 3.01539803 Saved: ~ Window Geometry: Displays: diff --git a/realsense_ros_camera/src/realsense_camera_node.cpp b/realsense_ros_camera/src/realsense_camera_node.cpp index 78266b25a0..83da6a268a 100644 --- a/realsense_ros_camera/src/realsense_camera_node.cpp +++ b/realsense_ros_camera/src/realsense_camera_node.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +22,8 @@ #include #include #include +#include + #define REALSENSE_ROS_EMBEDDED_VERSION_STR (VAR_ARG_STRING(VERSION: REALSENSE_ROS_MAJOR_VERSION.REALSENSE_ROS_MINOR_VERSION.REALSENSE_ROS_PATCH_VERSION)) constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR; @@ -45,7 +48,7 @@ namespace realsense_ros_camera inline void signalHandler(int signum) { - ROS_INFO("\"Ctrl+C\" is being pressed! Terminate RealSense Node..."); + ROS_INFO_STREAM(strsignal(signum) << " Signal is received! Terminate RealSense Node..."); ros::shutdown(); exit(signum); } @@ -54,6 +57,7 @@ namespace realsense_ros_camera { public: RealSenseCameraNodelet() : + _serial_no(""), _base_frame_id(""), _intialize_time_base(false) { @@ -137,6 +141,13 @@ namespace realsense_ros_camera _pnh = getPrivateNodeHandle(); + _pnh.param("enable_pointcloud", _pointcloud, POINTCLOUD); + _pnh.param("enable_sync", _sync_frames, SYNC_FRAMES); + if (_pointcloud) + _sync_frames = true; + + _pnh.param("serial_no", _serial_no); + _pnh.param("depth_width", _width[DEPTH], DEPTH_WIDTH); _pnh.param("depth_height", _height[DEPTH], DEPTH_HEIGHT); _pnh.param("depth_fps", _fps[DEPTH], DEPTH_FPS); @@ -219,9 +230,14 @@ namespace realsense_ros_camera _serial_no = _dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); ROS_INFO_STREAM("Device Serial No: " << _serial_no); + auto fw_ver = _dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION); + ROS_INFO_STREAM("Device FW version: " << fw_ver); + auto pid = _dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID); ROS_INFO_STREAM("Device Product ID: " << pid); + ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off")); + auto dev_sensors = _dev.query_sensors(); ROS_INFO_STREAM("Device Sensors: "); @@ -261,7 +277,7 @@ namespace realsense_ros_camera // Update "enable" map std::vector> streams(IMAGE_STREAMS); streams.insert(streams.end(), HID_STREAMS.begin(), HID_STREAMS.end()); - for (auto elem : streams) + for (auto& elem : streams) { for (auto& stream_index : elem) { @@ -294,7 +310,9 @@ namespace realsense_ros_camera { _image_publishers[DEPTH] = image_transport.advertise("camera/depth/image_raw", 1); _info_publisher[DEPTH] = _node_handle.advertise("camera/depth/camera_info", 1); - _pointcloud_publisher = _node_handle.advertise("/camera/points", 1); + + if (_pointcloud) + _pointcloud_publisher = _node_handle.advertise("/camera/points", 1); } if (true == _enable[INFRA1]) @@ -355,7 +373,7 @@ namespace realsense_ros_camera { auto& sens = _sensors[elem]; auto profiles = sens->get_stream_profiles(); - for (rs2::stream_profile& profile : profiles) + for (auto& profile : profiles) { auto video_profile = profile.as(); if (video_profile.format() == _format[elem] && @@ -366,7 +384,6 @@ namespace realsense_ros_camera { _enabled_profiles[elem].push_back(profile); - // Setup stream callback for stream _image[elem] = cv::Mat(_width[elem], _height[elem], _image_format[elem], cv::Scalar(0, 0, 0)); ROS_INFO_STREAM(_stream_name[elem] << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem]); break; @@ -396,6 +413,60 @@ namespace realsense_ros_camera } } + auto frame_callback = [this](rs2::frame frame) + { + // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, + // and the incremental timestamp from the camera. + // In sync mode the timestamp is based on ROS time + if (false == _intialize_time_base) + { + _intialize_time_base = true; + _ros_time_base = ros::Time::now(); + _camera_time_base = frame.get_timestamp(); + } + + ros::Time t; + if (_sync_frames) + t = ros::Time::now(); + else + t = ros::Time(_ros_time_base.toSec()+ (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000); + + auto is_color_frame_arrived = false; + auto is_depth_frame_arrived = false; + if (frame.is()) + { + ROS_DEBUG("Frameset arrived"); + auto frameset = frame.as(); + for (auto it = frameset.begin(); it != frameset.end(); ++it) + { + auto f = (*it); + auto stream_type = f.get_profile().stream_type(); + if (RS2_STREAM_COLOR == stream_type) + is_color_frame_arrived = true; + else if (RS2_STREAM_DEPTH == stream_type) + is_depth_frame_arrived = true; + + ROS_DEBUG("Frameset contain %s frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + rs2_stream_to_string(stream_type), frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); + publishFrame(f, t); + } + } + else + { + auto stream_type = frame.get_profile().stream_type(); + ROS_DEBUG("%s video frame arrived. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", + rs2_stream_to_string(stream_type), frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); + publishFrame(frame, t); + } + + if(_pointcloud && is_depth_frame_arrived && is_color_frame_arrived && + (0 != _pointcloud_publisher.getNumSubscribers())) + { + ROS_DEBUG("publishPCTopic(...)"); + publishPCTopic(t); + } + }; + // Streaming IMAGES for (auto& streams : IMAGE_STREAMS) { @@ -414,82 +485,29 @@ namespace realsense_ros_camera { auto stream = streams.front(); auto& sens = _sensors[stream]; - // Enable the stream sens->open(profiles); - if (stream == DEPTH) + if (DEPTH == stream) { auto depth_sensor = sens->as(); _depth_scale_meters = depth_sensor.get_depth_scale(); } - // Start device with the following callback - sens->start([this](rs2::frame frame) + if (_sync_frames) { - // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, - // and the incremental timestamp from the camera. - if (false == _intialize_time_base) - { - _intialize_time_base = true; - _ros_time_base = ros::Time::now(); - _camera_time_base = frame.get_timestamp(); - } - double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000; - ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); - - ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", - rs2_stream_to_string(frame.get_profile().stream_type()), - frame.get_profile().stream_index(), - rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); - - stream_index_pair stream{frame.get_profile().stream_type(), frame.get_profile().stream_index()}; - auto& image = _image[stream]; - image.data = (uint8_t*)frame.get_data(); - - if((DEPTH == stream) && (0 != _pointcloud_publisher.getNumSubscribers())) - publishPCTopic(t); - - ++(_seq[stream]); - auto& info_publisher = _info_publisher[stream]; - auto& image_publisher = _image_publishers[stream]; - if(0 != info_publisher.getNumSubscribers() || - 0 != image_publisher.getNumSubscribers()) - { - auto width = 0; - auto height = 0; - auto bpp = 1; - if (frame.is()) - { - auto image = frame.as(); - width = image.get_width(); - height = image.get_height(); - bpp = image.get_bytes_per_pixel(); - } - - sensor_msgs::ImagePtr img; - img = cv_bridge::CvImage(std_msgs::Header(), _encoding[stream], image).toImageMsg(); - img->width = width; - img->height = height; - img->is_bigendian = false; - img->step = width * bpp; - img->header.frame_id = _optical_frame_id[stream]; - img->header.stamp = t; - img->header.seq = _seq[stream]; - - auto& cam_info = _camera_info[stream]; - cam_info.header.stamp = t; - cam_info.header.seq = _seq[stream]; - info_publisher.publish(cam_info); - - image_publisher.publish(img); - ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); - return; - } - ROS_DEBUG("Skipping publish %s stream. Topic is not subscribed!", rs2_stream_to_string(frame.get_profile().stream_type())); - }); + sens->start(_syncer); + } + else + { + sens->start(frame_callback); + } } }//end for + if (_sync_frames) + { + _syncer.start(frame_callback); + } // Streaming HID for (const auto streams : HID_STREAMS) @@ -503,7 +521,7 @@ namespace realsense_ros_camera for (rs2::stream_profile& profile : profiles) { if (profile.fps() == _fps[elem] && - profile.format() == _format[elem]) + profile.format() == _format[elem]) { _enabled_profiles[elem].push_back(profile); break; @@ -642,15 +660,22 @@ namespace realsense_ros_camera _camera_info[stream_index].P.at(10) = 1; _camera_info[stream_index].P.at(11) = 0; - // TODO: Depth to Color? + rs2::stream_profile depth_profile; + if (!getEnabledProfile(DEPTH, depth_profile)) + { + ROS_ERROR_STREAM("Depth profile not found!"); + ros::shutdown(); + exit(1); + } + + // TODO: Why Depth to Color? if (stream_index == DEPTH && _enable[DEPTH] && _enable[COLOR]) { + _depth2color_extrinsics = depth_profile.get_extrinsics_to(_enabled_profiles[COLOR].front()); // set depth to color translation values in Projection matrix (P) - auto depth_profile = _enabled_profiles[DEPTH].front(); - auto d2c_extrinsics = depth_profile.get_extrinsics_to(_enabled_profiles[COLOR].front()); - _camera_info[stream_index].P.at(3) = d2c_extrinsics.translation[0]; // Tx - _camera_info[stream_index].P.at(7) = d2c_extrinsics.translation[1]; // Ty - _camera_info[stream_index].P.at(11) = d2c_extrinsics.translation[2]; // Tz + _camera_info[stream_index].P.at(3) = _depth2color_extrinsics.translation[0]; // Tx + _camera_info[stream_index].P.at(7) = _depth2color_extrinsics.translation[1]; // Ty + _camera_info[stream_index].P.at(11) = _depth2color_extrinsics.translation[2]; // Tz } _camera_info[stream_index].distortion_model = "plumb_bob"; @@ -672,9 +697,18 @@ namespace realsense_ros_camera } } + Eigen::Quaternionf rotationMatrixToQuaternion(float rotation[3]) const + { + Eigen::Matrix3f m; + m << rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8]; + Eigen::Quaternionf q(m); + return q; + } + void publishStaticTransforms() { - // TODO: Add the extrinsics rotation matrix ROS_INFO("publishStaticTransforms..."); // Publish transforms for the cameras tf::Quaternion q_c2co; @@ -725,24 +759,29 @@ namespace realsense_ros_camera d2do_msg.transform.rotation.w = q_d2do.getW(); _static_tf_broadcaster.sendTransform(d2do_msg); - // Assuming that all D400 SKUs have depth sensor - auto& sens = _sensors[DEPTH]; - auto depth_profile = sens->get_stream_profiles().front(); - if (true == _enable[COLOR]) + rs2::stream_profile depth_profile; + if (!getEnabledProfile(DEPTH, depth_profile)) { - auto d2c_extrinsics = depth_profile.get_extrinsics_to(_enabled_profiles[COLOR].front()); + ROS_ERROR_STREAM("Depth profile not found!"); + ros::shutdown(); + exit(1); + } + if (true == _enable[COLOR]) + { // Transform base frame to color frame + auto q = rotationMatrixToQuaternion(_depth2color_extrinsics.rotation); + b2c_msg.header.stamp = transform_ts_; b2c_msg.header.frame_id = _base_frame_id; b2c_msg.child_frame_id = _frame_id[COLOR]; - b2c_msg.transform.translation.x = d2c_extrinsics.translation[2]; - b2c_msg.transform.translation.y = -d2c_extrinsics.translation[0]; - b2c_msg.transform.translation.z = -d2c_extrinsics.translation[1]; - b2c_msg.transform.rotation.x = 0; - b2c_msg.transform.rotation.y = 0; - b2c_msg.transform.rotation.z = 0; - b2c_msg.transform.rotation.w = 1; + b2c_msg.transform.translation.x = _depth2color_extrinsics.translation[2]; + b2c_msg.transform.translation.y = -_depth2color_extrinsics.translation[0]; + b2c_msg.transform.translation.z = -_depth2color_extrinsics.translation[1]; + b2c_msg.transform.rotation.x = q.x(); + b2c_msg.transform.rotation.y = q.y(); + b2c_msg.transform.rotation.z = q.z(); + b2c_msg.transform.rotation.w = q.w(); _static_tf_broadcaster.sendTransform(b2c_msg); // Transform color frame to color optical frame @@ -763,6 +802,7 @@ namespace realsense_ros_camera if (true == _enable[INFRA1]) { auto d2ir1_extrinsics = depth_profile.get_extrinsics_to(_enabled_profiles[INFRA1].front()); + auto q = rotationMatrixToQuaternion(d2ir1_extrinsics.rotation); // Transform base frame to infra1 b2ir1_msg.header.stamp = transform_ts_; @@ -771,10 +811,11 @@ namespace realsense_ros_camera b2ir1_msg.transform.translation.x = d2ir1_extrinsics.translation[2]; b2ir1_msg.transform.translation.y = -d2ir1_extrinsics.translation[0]; b2ir1_msg.transform.translation.z = -d2ir1_extrinsics.translation[1]; - b2ir1_msg.transform.rotation.x = 0; - b2ir1_msg.transform.rotation.y = 0; - b2ir1_msg.transform.rotation.z = 0; - b2ir1_msg.transform.rotation.w = 1; + + b2ir1_msg.transform.rotation.x = q.x(); + b2ir1_msg.transform.rotation.y = q.y(); + b2ir1_msg.transform.rotation.z = q.z(); + b2ir1_msg.transform.rotation.w = q.w(); _static_tf_broadcaster.sendTransform(b2ir1_msg); // Transform infra1 frame to infra1 optical frame @@ -795,6 +836,7 @@ namespace realsense_ros_camera if (true == _enable[INFRA2]) { auto d2ir2_extrinsics = depth_profile.get_extrinsics_to(_enabled_profiles[INFRA2].front()); + auto q = rotationMatrixToQuaternion(d2ir2_extrinsics.rotation); // Transform base frame to infra2 b2ir2_msg.header.stamp = transform_ts_; @@ -803,10 +845,10 @@ namespace realsense_ros_camera b2ir2_msg.transform.translation.x = d2ir2_extrinsics.translation[2]; b2ir2_msg.transform.translation.y = -d2ir2_extrinsics.translation[0]; b2ir2_msg.transform.translation.z = -d2ir2_extrinsics.translation[1]; - b2ir2_msg.transform.rotation.x = 0; - b2ir2_msg.transform.rotation.y = 0; - b2ir2_msg.transform.rotation.z = 0; - b2ir2_msg.transform.rotation.w = 1; + b2ir2_msg.transform.rotation.x = q.x(); + b2ir2_msg.transform.rotation.y = q.y(); + b2ir2_msg.transform.rotation.z = q.z(); + b2ir2_msg.transform.rotation.w = q.w(); _static_tf_broadcaster.sendTransform(b2ir2_msg); // Transform infra2 frame to infra1 optical frame @@ -828,57 +870,84 @@ namespace realsense_ros_camera void publishPCTopic(const ros::Time& t) { - auto& depth_intrinsic = _stream_intrinsics[DEPTH]; - auto& depth_image = _image[DEPTH]; + auto color_intrinsics = _stream_intrinsics[COLOR]; + auto image_depth16 = reinterpret_cast(_image[DEPTH].data); + auto depth_intrinsics = _stream_intrinsics[DEPTH]; sensor_msgs::PointCloud2 msg_pointcloud; msg_pointcloud.header.stamp = t; msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; - msg_pointcloud.width = depth_intrinsic.width; - msg_pointcloud.height = depth_intrinsic.height; + msg_pointcloud.width = depth_intrinsics.width; + msg_pointcloud.height = depth_intrinsics.height; msg_pointcloud.is_dense = true; sensor_msgs::PointCloud2Modifier modifier(msg_pointcloud); - modifier.setPointCloud2Fields(3, + + modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32); - modifier.setPointCloud2FieldsByString(1, "xyz"); + "z", 1, sensor_msgs::PointField::FLOAT32, + "rgb", 1, sensor_msgs::PointField::FLOAT32); + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - for (int v = 0; v < depth_intrinsic.height; ++v) + sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); + + sensor_msgs::PointCloud2Iteratoriter_r(msg_pointcloud, "r"); + sensor_msgs::PointCloud2Iteratoriter_g(msg_pointcloud, "g"); + sensor_msgs::PointCloud2Iteratoriter_b(msg_pointcloud, "b"); + + float depth_point[3], color_point[3], color_pixel[2], scaled_depth; + unsigned char* color_data = _image[COLOR].data; + + // Fill the PointCloud2 fields + for (int y = 0; y < depth_intrinsics.height; ++y) { - for (int u = 0; u < depth_intrinsic.width; ++u) + for (int x = 0; x < depth_intrinsics.width; ++x) { - float depth_point[3], scaled_depth; - uint16_t depth_value; - int depth_offset, cloud_offset; - - // Offset into point cloud data, for point at u, v - cloud_offset = (v * msg_pointcloud.row_step) + (u * msg_pointcloud.point_step); - - // Retrieve depth value, and scale it in terms of meters - depth_offset = (u * sizeof(uint16_t)) + (v * sizeof(uint16_t) * depth_intrinsic.width); - memcpy(&depth_value, &depth_image.data[depth_offset], sizeof(uint16_t)); - scaled_depth = static_cast(depth_value) * _depth_scale_meters; - if (scaled_depth <= 0.0f || scaled_depth > 5.0) // TODO: move to constants + 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] <= 0.f || depth_point[2] > 5.f) { - // Depth value is invalid, so zero it out. - depth_point[0] = 0.0f; - depth_point[1] = 0.0f; - depth_point[2] = 0.0f; + depth_point[0] = 0.f; + depth_point[1] = 0.f; + depth_point[2] = 0.f; } - else + + *iter_x = depth_point[0]; + *iter_y = depth_point[1]; + *iter_z = depth_point[2]; + + rs2_transform_point_to_point(color_point, &_depth2color_extrinsics, depth_point); + rs2_project_point_to_pixel(color_pixel, &color_intrinsics, color_point); + + if (color_pixel[1] < 0.f || color_pixel[1] > color_intrinsics.height + || color_pixel[0] < 0.f || color_pixel[0] > color_intrinsics.width) { - // Convert depth image to points in 3D space - float depth_pixel[2] = {static_cast(u), static_cast(v)}; - rs2_deproject_pixel_to_point(depth_point, &depth_intrinsic, depth_pixel, scaled_depth); + // For out of bounds color data, default to a shade of blue in order to visually distinguish holes. + // This color value is same as the librealsense out of bounds color value. + *iter_r = static_cast(96); + *iter_g = static_cast(157); + *iter_b = static_cast(198); } + else + { + auto i = static_cast(color_pixel[0]); + auto j = static_cast(color_pixel[1]); - // Assign 3d point - for (int i = 0; i < 3; ++i) - memcpy(&msg_pointcloud.data[cloud_offset + msg_pointcloud.fields[i].offset], &depth_point[i], sizeof(float)); - } // for - } // for + auto offset = i * 3 + j * color_intrinsics.width * 3; + *iter_r = static_cast(color_data[offset]); + *iter_g = static_cast(color_data[offset + 1]); + *iter_b = static_cast(color_data[offset + 2]); + } + ++image_depth16; + ++iter_x; ++iter_y; ++iter_z; + ++iter_r; ++iter_g; ++iter_b; + } + } _pointcloud_publisher.publish(msg_pointcloud); } @@ -968,6 +1037,63 @@ namespace realsense_ros_camera } } + void publishFrame(rs2::frame f, const ros::Time& t) + { + ROS_DEBUG("publishFrame(...)"); + stream_index_pair stream{f.get_profile().stream_type(), f.get_profile().stream_index()}; + auto& image = _image[stream]; + image.data = (uint8_t*)f.get_data(); + ++(_seq[stream]); + auto& info_publisher = _info_publisher[stream]; + auto& image_publisher = _image_publishers[stream]; + if(0 != info_publisher.getNumSubscribers() || + 0 != image_publisher.getNumSubscribers()) + { + auto width = 0; + auto height = 0; + auto bpp = 1; + if (f.is()) + { + auto image = f.as(); + width = image.get_width(); + height = image.get_height(); + bpp = image.get_bytes_per_pixel(); + } + + sensor_msgs::ImagePtr img; + img = cv_bridge::CvImage(std_msgs::Header(), _encoding[stream], image).toImageMsg(); + img->width = width; + img->height = height; + img->is_bigendian = false; + img->step = width * bpp; + img->header.frame_id = _optical_frame_id[stream]; + img->header.stamp = t; + img->header.seq = _seq[stream]; + + auto& cam_info = _camera_info[stream]; + cam_info.header.stamp = t; + cam_info.header.seq = _seq[stream]; + info_publisher.publish(cam_info); + + image_publisher.publish(img); + ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); + } + } + + bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile) + { + // Assuming that all D400 SKUs have depth sensor + auto profiles = _enabled_profiles[stream_index]; + auto it = std::find_if(profiles.begin(), profiles.end(), + [&](const rs2::stream_profile& profile) + { return (profile.stream_type() == stream_index.first); }); + if (it == profiles.end()) + return false; + + profile = *it; + return true; + } + ros::NodeHandle _node_handle, _pnh; std::unique_ptr _ctx; rs2::device _dev; @@ -1006,6 +1132,10 @@ namespace realsense_ros_camera ros::Publisher _pointcloud_publisher; ros::Time _ros_time_base; + bool _sync_frames; + bool _pointcloud; + rs2::asynchronous_syncer _syncer; + rs2_extrinsics _depth2color_extrinsics; };//end class PLUGINLIB_EXPORT_CLASS(realsense_ros_camera::RealSenseCameraNodelet, nodelet::Nodelet)