PR #2409 from Samer: fix rosbag device loading by preventing set_opti…

…on to HDR/Gain/Exposure
IntelRealSense · Jul 17, 2022 · c1a5c45 · c1a5c45
2 parents 505c23b + 7ffa714
commit c1a5c45
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 10 deletions.
diff --git a/realsense2_camera/include/ros_sensor.h b/realsense2_camera/include/ros_sensor.h
@@ -70,7 +70,8 @@ namespace realsense2_camera
                       std::function<void()> hardware_reset_func, 
                       std::shared_ptr<diagnostic_updater::Updater> diagnostics_updater,
                       rclcpp::Logger logger,
-                      bool force_image_default_qos = false);
+                      bool force_image_default_qos = false,
+                      bool is_rosbag_file = false);
             ~RosSensor();
             void registerSensorParameters();
             bool getUpdatedProfiles(std::vector<rs2::stream_profile>& wanted_profiles);
@@ -88,7 +89,7 @@ namespace realsense2_camera
 
         private:
             void setupErrorCallback();
-            void setParameters();
+            void setParameters(bool is_rosbag_file = false);
             void clearParameters();
             void set_sensor_auto_exposure_roi();
             void registerAutoExposureROIOptions();

diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp
@@ -33,7 +33,8 @@ RosSensor::RosSensor(rs2::sensor sensor,
     std::function<void()> hardware_reset_func, 
     std::shared_ptr<diagnostic_updater::Updater> diagnostics_updater,
     rclcpp::Logger logger,
-    bool force_image_default_qos):
+    bool force_image_default_qos,
+    bool is_rosbag_file):
     rs2::sensor(sensor),
     _logger(logger),
     _origin_frame_callback(frame_callback),
@@ -61,7 +62,7 @@ RosSensor::RosSensor(rs2::sensor sensor,
                 ROS_ERROR_STREAM("An error has occurred during frame callback: " << ex.what());
             }
         };
-    setParameters();
+    setParameters(is_rosbag_file);
 }
 
 RosSensor::~RosSensor()
@@ -70,11 +71,16 @@ RosSensor::~RosSensor()
     stop();
 }
 
-void RosSensor::setParameters()
+void RosSensor::setParameters(bool is_rosbag_file)
 {
     std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)));
     _params.registerDynamicOptions(*this, module_name);
-    UpdateSequenceIdCallback();
+
+    // for rosbag files, don't set hdr(sequence_id) / gain / exposure options
+    // since these options can be changed only in real devices
+    if(!is_rosbag_file)
+        UpdateSequenceIdCallback();
+
     registerSensorParameters();
 }
 

diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp
@@ -97,7 +97,7 @@ void BaseRealSenseNode::setAvailableSensors()
     ROS_INFO_STREAM("Device Product ID: 0x" << pid);
 
     ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off"));
-
+    
     std::function<void(rs2::frame)> frame_callback_function = [this](rs2::frame frame){
         bool is_filter(_filters.end() != find_if(_filters.begin(), _filters.end(), [](std::shared_ptr<NamedFilter> f){return (f->is_enabled()); }));
         if (_sync_frames || is_filter)
@@ -135,17 +135,17 @@ void BaseRealSenseNode::setAvailableSensors()
             sensor.is<rs2::fisheye_sensor>())
         {
             ROS_DEBUG_STREAM("Set " << module_name << " as VideoSensor.");
-            rosSensor = std::make_unique<RosSensor>(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process);
+            rosSensor = std::make_unique<RosSensor>(sensor, _parameters, frame_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, _use_intra_process, _dev.is<playback>());
         }
         else if (sensor.is<rs2::motion_sensor>())
         {
             ROS_DEBUG_STREAM("Set " << module_name << " as ImuSensor.");
-            rosSensor = std::make_unique<RosSensor>(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger);
+            rosSensor = std::make_unique<RosSensor>(sensor, _parameters, imu_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is<playback>());
         }
         else if (sensor.is<rs2::pose_sensor>())
         {
             ROS_DEBUG_STREAM("Set " << module_name << " as PoseSensor.");
-            rosSensor = std::make_unique<RosSensor>(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger);
+            rosSensor = std::make_unique<RosSensor>(sensor, _parameters, multiple_message_callback_function, update_sensor_func, hardware_reset_func, _diagnostics_updater, _logger, false, _dev.is<playback>());
         }
         else
         {