Set service mode before updating controls

Arun-Prasad-V · Mar 7, 2024 · 705d056 · 705d056
1 parent ec7c093
commit 705d056
Show file tree

Hide file tree

Showing 2 changed files with 35 additions and 2 deletions.
diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py
@@ -82,6 +82,7 @@
                            {'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'},
                            {'name': 'enable_safety',                'default': 'false', 'description': "'enable safety stream'"},                                                      
+                           {'name': 'safety_camera.safety_mode',    'default': '0', 'description': '[int] 0-Run, 1-Standby, 2-Service'},                                                       
                            {'name': 'enable_labeled_point_cloud',   'default': 'false', 'description': "'enable labeled point cloud stream'"},
                            {'name': 'enable_occupancy',             'default': 'false', 'description': "'enable occupancy stream'"},
                            {'name': 'depth_mapping_camera.profile', 'default': '0,0,0', 'description': "'depth mapping camera profile'"},

diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp
@@ -130,13 +130,45 @@ void BaseRealSenseNode::setAvailableSensors()
     std::function<void()> hardware_reset_func = [this](){hardwareResetRequest();};
 
     _dev_sensors = _dev.query_sensors();
+    rs2::sensor* safety_sensor = nullptr;
+    auto safety_mode = RS2_SAFETY_MODE_RUN;
+
+    // Get the Safety Sensor.
+    auto pivot = std::find_if(_dev_sensors.begin(), _dev_sensors.end(), 
+                            [](rs2::sensor sensor){return sensor.is<rs2::safety_sensor>();});
+    if (pivot != _dev_sensors.end())
+    {
+        safety_sensor = &(*pivot);
+    }
 
     for(auto&& sensor : _dev_sensors)
     {
         const std::string module_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME)));
         std::unique_ptr<RosSensor> rosSensor;
-        if (sensor.is<rs2::depth_sensor>() || 
-            sensor.is<rs2::color_sensor>() ||
+        if (sensor.is<rs2::depth_sensor>())
+        {
+            auto deleter_to_revert_safety_mode = std::unique_ptr<rs2_safety_mode, std::function<void(rs2_safety_mode*)>>(&safety_mode, 
+                                            [&](rs2_safety_mode* revert_safety_mode_to){
+                                                    if (revert_safety_mode_to && safety_sensor)
+                                                    {
+                                                        ROS_INFO("Arun3: Reverting from service mode" );
+                                                        safety_sensor->set_option(RS2_OPTION_SAFETY_MODE, *revert_safety_mode_to);
+                                                    }
+                                                });
+            if (safety_sensor)
+            {
+                safety_mode = static_cast<rs2_safety_mode>(safety_sensor->get_option(RS2_OPTION_SAFETY_MODE));
+                if (safety_mode != RS2_SAFETY_MODE_SERVICE)
+                {
+                    ROS_INFO("Arun1: Setting to service mode.");
+                    safety_sensor->set_option(RS2_OPTION_SAFETY_MODE, RS2_SAFETY_MODE_SERVICE);
+                }
+            }
+
+            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, _dev.is<playback>());
+        }
+        else if (sensor.is<rs2::color_sensor>() ||
             sensor.is<rs2::safety_sensor>() ||
             sensor.is<rs2::depth_mapping_sensor>())
         {