PR #2830 from SamerKhshiboun: Add RGBD + reduce changes between hkr a…

…nd development
IntelRealSense · Aug 3, 2023 · 5b5dbc8 · 5b5dbc8
2 parents 6a2a0fc + cddce69
commit 5b5dbc8
Show file tree

Hide file tree

Showing 11 changed files with 305 additions and 76 deletions.
diff --git a/README.md b/README.md
@@ -32,6 +32,7 @@
      * [TF from coordinate A to coordinate B](#tfs)
      * [Extrinsics from sensor A to sensor B](#extrinsics)
      * [Topics](#topics)
+     * [RGBD Topic](#rgbd)
      * [Metadata Topic](#metadata)
      * [Post-Processing Filters](#filters)
      * [Available Services](#services)
@@ -416,6 +417,28 @@ Enabling stream adds matching topics. For instance, enabling the gyro and accel
 
 <hr>
 
+<h3 id="rgbd">
+  RGBD Topic
+</h3>
+
+RGBD new topic, publishing [RGB + Depth] in the same message (see RGBD.msg for reference). For now, works only with depth aligned to color images, as color and depth images are synchronized by frame time tag.
+
+These boolean paramters should be true to enable rgbd messages:
+
+- `enable_rgbd`: new paramter, to enable/disable rgbd topic, changeable during runtime
+- `align_depth.enable`: align depth images to rgb images
+- `enable_sync`: let librealsense sync between frames, and get the frameset with color and depth images combined
+- `enable_color` + `enable_depth`: enable both color and depth sensors
+
+The current QoS of the topic itself, is the same as Depth and Color streams (SYSTEM_DEFAULT)
+
+Example:
+```
+ros2 launch realsense2_camera rs_launch.py enable_rgbd:=true enable_sync:=true align_depth.enable:=true enable_color:=true enable_depth:=true 
+```
+
+<hr>
+
 <h3 id="metadata">
   Metadata topic
 </h3>

diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
@@ -30,6 +30,7 @@
 #include "realsense2_camera_msgs/msg/imu_info.hpp"
 #include "realsense2_camera_msgs/msg/extrinsics.hpp"
 #include "realsense2_camera_msgs/msg/metadata.hpp"
+#include "realsense2_camera_msgs/msg/rgbd.hpp"
 #include "realsense2_camera_msgs/srv/device_info.hpp"
 #include <librealsense2/hpp/rs_processing.hpp>
 #include <librealsense2/rs_advanced_mode.hpp>
@@ -57,6 +58,7 @@
 
 using realsense2_camera_msgs::msg::Extrinsics;
 using realsense2_camera_msgs::msg::IMUInfo;
+using realsense2_camera_msgs::msg::RGBD;
 
 #define FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_frame")).str()
 #define IMU_FRAME_ID (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_imu_frame")).str()
@@ -206,14 +208,34 @@ namespace realsense2_camera
 
         IMUInfo getImuInfo(const rs2::stream_profile& profile);
 
-        void publishFrame(rs2::frame f,
+        void fillMessageImage(
+            const cv::Mat& cv_matrix_image,
+            const stream_index_pair& stream,
+            unsigned int width,
+            unsigned int height,
+            unsigned int bpp,
+            const rclcpp::Time& t,
+            sensor_msgs::msg::Image* img_msg_ptr);
+
+        cv::Mat& getCVMatImage(
+            rs2::frame& frame,
+            std::map<stream_index_pair, cv::Mat>& images,
+            unsigned int width,
+            unsigned int height,
+            unsigned int bpp,
+            const stream_index_pair& stream);
+
+        void publishFrame(
+            rs2::frame f,
             const rclcpp::Time& t,
             const stream_index_pair& stream,
             std::map<stream_index_pair, cv::Mat>& images,
             const std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>& info_publishers,
             const std::map<stream_index_pair, std::shared_ptr<image_publisher>>& image_publishers,
             const bool is_publishMetadata = true);
 
+        void publishRGBD(const cv::Mat& rgb_cv_matrix, const cv::Mat& depth_cv_matrix, const rclcpp::Time& t);
+
         void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id);
 
         sensor_msgs::msg::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data);
@@ -236,6 +258,7 @@ namespace realsense2_camera
         void updateSensors();
         void publishServices();
         void startPublishers(const std::vector<rs2::stream_profile>& profiles, const RosSensor& sensor);
+        void startRGBDPublisherIfNeeded();
         void stopPublishers(const std::vector<rs2::stream_profile>& profiles);
 
         rs2::device _dev;
@@ -275,6 +298,7 @@ namespace realsense2_camera
         std::map<stream_index_pair, rclcpp::Publisher<realsense2_camera_msgs::msg::Metadata>::SharedPtr> _metadata_publishers;
         std::map<stream_index_pair, rclcpp::Publisher<IMUInfo>::SharedPtr> _imu_info_publishers;
         std::map<stream_index_pair, rclcpp::Publisher<Extrinsics>::SharedPtr> _extrinsics_publishers;
+        rclcpp::Publisher<realsense2_camera_msgs::msg::RGBD>::SharedPtr _rgbd_publisher;
         std::map<stream_index_pair, cv::Mat> _images;
         std::map<unsigned int, std::string> _encoding;
 
@@ -284,6 +308,9 @@ namespace realsense2_camera
 
         rclcpp::Time _ros_time_base;
         bool _sync_frames;
+        bool _enable_rgbd;
+        bool _is_color_enabled;
+        bool _is_depth_enabled;
         bool _pointcloud;
         bool _publish_odom_tf;
         imu_sync_method _imu_sync_method;

diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h
@@ -89,6 +89,7 @@ namespace realsense2_camera
     const bool ALLOW_NO_TEXTURE_POINTS = false;
     const bool ORDERED_PC     = false;
     const bool SYNC_FRAMES    = false;
+    const bool ENABLE_RGBD    = false;
 
     const bool PUBLISH_TF     = true;
     const double TF_PUBLISH_RATE = 0; // Static transform

diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py
@@ -48,15 +48,16 @@
                            {'name': 'enable_confidence',            'default': 'true', 'description': 'enable confidence'},
                            {'name': 'gyro_fps',                     'default': '0', 'description': "''"},                           
                            {'name': 'accel_fps',                    'default': '0', 'description': "''"},                           
-                           {'name': 'enable_gyro',                  'default': 'false', 'description': "''"},                           
-                           {'name': 'enable_accel',                 'default': 'false', 'description': "''"},                           
-                           {'name': 'enable_pose',                  'default': 'true', 'description': "''"},                           
+                           {'name': 'enable_gyro',                  'default': 'false', 'description': "'enable gyro stream'"},                           
+                           {'name': 'enable_accel',                 'default': 'false', 'description': "'enable accel stream'"},                           
+                           {'name': 'enable_pose',                  'default': 'true', 'description': "'enable pose stream'"},                           
                            {'name': 'pose_fps',                     'default': '200', 'description': "''"},                           
-                           {'name': 'pointcloud.enable',            'default': 'false', 'description': ''}, 
+                           {'name': 'pointcloud.enable',            'default': 'false', 'description': ''},
                            {'name': 'pointcloud.stream_filter',     'default': '2', 'description': 'texture stream for pointcloud'},
                            {'name': 'pointcloud.stream_index_filter','default': '0', 'description': 'texture stream index for pointcloud'},
-                           {'name': 'enable_sync',                  'default': 'false', 'description': "''"},                           
-                           {'name': 'align_depth.enable',           'default': 'false', 'description': "''"},                           
+                           {'name': 'enable_sync',                  'default': 'false', 'description': "'enable sync mode'"},
+                           {'name': 'enable_rgbd',                  'default': 'false', 'description': "'enable rgbd topic'"},
+                           {'name': 'align_depth.enable',           'default': 'false', 'description': "'enable align depth filter'"},
                            {'name': 'colorizer.enable',             'default': 'false', 'description': "''"},
                            {'name': 'clip_distance',                'default': '-2.', 'description': "''"},                           
                            {'name': 'linear_accel_cov',             'default': '0.01', 'description': "''"},