PR #2755 from SamerKhshiboun: Rebase ros2-hkr

IntelRealSense · May 29, 2023 · 65c480b · 65c480b
2 parents b1efa89 + 8ee06f7
commit 65c480b
Show file tree

Hide file tree

Showing 23 changed files with 846 additions and 482 deletions.
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
@@ -23,7 +23,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distro: [rolling, humble, galactic, foxy, eloquent, dashing]
+        ros_distro: [rolling, humble, galactic, foxy]
         include:
         - ros_distro: 'rolling'
           os: ubuntu-22.04
@@ -33,10 +33,6 @@ jobs:
           os: ubuntu-20.04
         - ros_distro: 'foxy'
           os: ubuntu-20.04
-        - ros_distro: 'eloquent'
-          os: ubuntu-18.04
-        - ros_distro: 'dashing'
-          os: ubuntu-18.04
 
     steps:
 
@@ -63,12 +59,12 @@ jobs:
       with:
         cmake-version: '3.23.0'
 
-    ## If distro is one of [galactic, foxy, eloquent, dashing], use the [email protected]
+    ## If distro is one of [galactic, foxy], use the [email protected]
     ## which supports old versions of ROS2 and EOL versions.
     ## For Humble distro, use v0.3 
     ## See: 1) https://github.com/ros-tooling/setup-ros#Supported-platforms
     ##      2) https://github.com/ros-tooling/setup-ros/tree/v0.2#Supported-platforms 
-    - name: build ROS2 Galactic/Foxy/Eloquent/Dashing
+    - name: build ROS2 Galactic/Foxy
       if: ${{ matrix.ros_distro != 'humble' && matrix.ros_distro != 'rolling'}}
       uses: ros-tooling/[email protected]
       with:

diff --git a/README.md b/README.md
diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt
@@ -138,6 +138,7 @@ set(SOURCES
     src/named_filter.cpp
     src/profile_manager.cpp
     src/image_publisher.cpp
+    src/tfs.cpp
   )
 
 if(NOT DEFINED ENV{ROS_DISTRO})

diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
@@ -155,16 +155,20 @@ namespace realsense2_camera
         std::shared_ptr<Parameters> _parameters;
         std::list<std::string> _parameters_names;
 
+        void restartStaticTransformBroadcaster();
         void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex);
-        void calcAndPublishStaticTransform(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile);
+        void calcAndAppendTransformMsgs(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile);
         void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req,
                                  realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res);
         tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const;
-        void publish_static_tf(const rclcpp::Time& t,
+        void append_static_tf_msg(const rclcpp::Time& t,
                                const float3& trans,
                                const tf2::Quaternion& q,
                                const std::string& from,
                                const std::string& to);
+        void erase_static_tf_msg(const std::string& frame_id,
+                                 const std::string& child_frame_id);
+        void eraseTransformMsgs(const stream_index_pair& sip, const rs2::stream_profile& profile);
         void setup();
 
     private:
@@ -192,14 +196,15 @@ namespace realsense2_camera
         void enable_devices();
         void setupFilters();
         bool setBaseTime(double frame_time, rs2_timestamp_domain time_domain);
+        uint64_t millisecondsToNanoseconds(double timestamp_ms);
         rclcpp::Time frameSystemTimeSec(rs2::frame frame);
         cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image);
         void clip_depth(rs2::depth_frame depth_frame, float clipping_dist);
         void updateProfilesStreamCalibData(const std::vector<rs2::stream_profile>& profiles);
         void updateExtrinsicsCalibData(const rs2::video_stream_profile& left_video_profile, const rs2::video_stream_profile& right_video_profile);
         void updateStreamCalibData(const rs2::video_stream_profile& video_profile);
         void SetBaseStream();
-        void publishStaticTransforms(std::vector<rs2::stream_profile> profiles);
+        void publishStaticTransforms();
         void startDynamicTf();
         void publishDynamicTransforms();
         void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);

diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h
@@ -77,6 +77,7 @@ namespace realsense2_camera
     const uint16_t RS435i_RGB_PID   = 0x0B3A; // AWGC_MM
     const uint16_t RS465_PID        = 0x0b4d; // D465
     const uint16_t RS416_RGB_PID    = 0x0B52; // F416 RGB
+    const uint16_t RS430i_PID       = 0x0b4b; // D430i
     const uint16_t RS405_PID        = 0x0B5B; // DS5U
     const uint16_t RS455_PID        = 0x0B5C; // D455
     const uint16_t RS457_PID        = 0xABCD; // D457

diff --git a/realsense2_camera/launch/rs_intra_process_demo_launch.py b/realsense2_camera/launch/rs_intra_process_demo_launch.py
@@ -54,6 +54,8 @@
                            {'name': 'enable_gyro',                  'default': 'false', 'description': "enable gyro stream"},                           
                            {'name': 'enable_accel',                 'default': 'false', 'description': "enable accel stream"}, 
                            {'name': 'intra_process_comms',          'default': 'true', 'description': "enable intra-process communication"}, 
+                           {'name': 'publish_tf',                   'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'},
+                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': '[double] rate in HZ for publishing dynamic TF'},
                           ]
 
 def declare_configurable_parameters(parameters):

diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py
@@ -33,6 +33,7 @@
                            {'name': 'depth_module.profile',         'default': '0,0,0', 'description': 'depth module profile'},                           
                            {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
                            {'name': 'rgb_camera.profile',           'default': '0,0,0', 'description': 'color image width'},
+                           {'name': 'rgb_camera.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for color image'},
                            {'name': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
                            {'name': 'enable_infra1',                'default': 'false', 'description': 'enable infra1 stream'},
                            {'name': 'enable_infra2',                'default': 'false', 'description': 'enable infra2 stream'},
@@ -58,15 +59,18 @@
                            {'name': 'initial_reset',                'default': 'false', 'description': "''"},                           
                            {'name': 'allow_no_texture_points',      'default': 'false', 'description': "''"},                           
                            {'name': 'pointcloud.ordered_pc',        'default': 'false', 'description': ''},
-                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': 'Rate of publishing static_tf'},
+                           {'name': 'publish_tf',                   'default': 'true', 'description': '[bool] enable/disable publishing static & dynamic TF'},
+                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': '[double] rate in Hz for publishing dynamic TF'},
                            {'name': 'diagnostics_period',           'default': '0.0', 'description': 'Rate of publishing diagnostics. 0=Disabled'},
                            {'name': 'decimation_filter.enable',     'default': 'false', 'description': 'Rate of publishing static_tf'},
                            {'name': 'rosbag_filename',              'default': "''", 'description': 'A realsense bagfile to run from as a device'},
                            {'name': 'depth_module.exposure.1',      'default': '7500', 'description': 'Depth module first exposure value. Used for hdr_merge filter'},
                            {'name': 'depth_module.gain.1',          'default': '16', 'description': 'Depth module first gain value. Used for hdr_merge filter'},
                            {'name': 'depth_module.exposure.2',      'default': '1', 'description': 'Depth module second exposure value. Used for hdr_merge filter'},
                            {'name': 'depth_module.gain.2',          'default': '16', 'description': 'Depth module second gain value. Used for hdr_merge filter'},
+                           {'name': 'depth_module.exposure',        'default': '8500', 'description': 'Depth module manual exposure value'},
                            {'name': 'depth_module.hdr_enabled',     'default': 'false', 'description': 'Depth module hdr enablement flag. Used for hdr_merge filter'},
+                           {'name': 'depth_module.enable_auto_exposure', 'default': 'true', 'description': 'enable/disable auto exposure for depth image'},
                            {'name': 'hdr_merge.enable',             'default': 'false', 'description': 'hdr_merge filter enablement flag'},
                            {'name': 'wait_for_device_timeout',      'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},
                            {'name': 'reconnect_timeout',            'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},