Set service mode before updating controls

Arun-Prasad-V · Mar 7, 2024 · c4f1437 · c4f1437
1 parent ec7c093
commit c4f1437
Showing 1 changed file with 25 additions and 0 deletions.
diff --git a/realsense2_camera/src/ros_sensor.cpp b/realsense2_camera/src/ros_sensor.cpp
@@ -86,6 +86,11 @@ void RosSensor::setParameters(bool is_rosbag_file)
 {
     std::string module_name = create_graph_resource_name(rs2_to_ros(get_info(RS2_CAMERA_INFO_NAME)));
 
+    if((!is_rosbag_file) && supports(RS2_OPTION_SAFETY_MODE))
+    {
+        set_option(RS2_OPTION_SAFETY_MODE, RS2_SAFETY_MODE_SERVICE);
+    }
+
     // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted.
     // So, during init of the node, forcefully disabling the HDR upfront and update it with new values.
     if((!is_rosbag_file) && supports(RS2_OPTION_HDR_ENABLED))
@@ -112,6 +117,21 @@ void RosSensor::UpdateSequenceIdCallback()
 
     bool is_hdr_enabled = static_cast<bool>(get_option(RS2_OPTION_HDR_ENABLED));
 
+    rs2_safety_mode safety_mode = static_cast<rs2_safety_mode>(get_option(RS2_OPTION_SAFETY_MODE));
+
+    // Deleter to revert back the RS2_OPTION_HDR_ENABLED value at the end.
+    auto deleter_to_revert_safety_mode = std::unique_ptr<rs2_safety_mode, std::function<void(rs2_safety_mode*)>>(&safety_mode,
+                                                [&](rs2_safety_mode* org_safety_mode) {
+                                                    if (org_safety_mode)
+                                                    {
+                                                        rs2_safety_mode current_safety_mode = static_cast<rs2_safety_mode>(get_option(RS2_OPTION_SAFETY_MODE));
+                                                        if (current_safety_mode != *org_safety_mode)
+                                                        {
+                                                            set_option(RS2_OPTION_SAFETY_MODE, *org_safety_mode);
+                                                        }
+                                                    }
+                                                });
+
     // Deleter to revert back the RS2_OPTION_HDR_ENABLED value at the end.
     auto deleter_to_revert_hdr = std::unique_ptr<bool, std::function<void(bool*)>>(&is_hdr_enabled,
                                                 [&](bool* enable_back_hdr) {
@@ -121,6 +141,11 @@ void RosSensor::UpdateSequenceIdCallback()
                                                     }
                                                 });
 
+    if (safety_mode != RS2_SAFETY_MODE_SERVICE)
+    {
+        set_option(RS2_OPTION_SAFETY_MODE, RS2_SAFETY_MODE_SERVICE);
+    }
+
     // From FW version 5.14.x.x, if HDR is enabled, updating UVC controls like exposure, gain , etc are restricted.
     // So, disable it before updating.
     if (is_hdr_enabled)