hw_reset implementation

README.md

@@ -298,6 +298,7 @@ User can set the camera name and camera namespace, to distinguish between camera
   /robot1/D455_1/imu
 
   > ros2 service list
+  /robot1/D455_1/hw_reset
   /robot1/D455_1/device_info
   ```
 
@@ -320,6 +321,7 @@ User can set the camera name and camera namespace, to distinguish between camera
 /camera/camera/imu
 
 > ros2 service list
+/camera/camera/hw_reset
 /camera/camera/device_info
 ```
 
@@ -639,6 +641,10 @@ Each of the above filters have it's own parameters, following the naming convent
 
 ## Available services
 
+### hw_reset:
+  - reset the device. The call stops all the streams too.
+  - Call example: `ros2 service call /camera/camera/hw_reset std_srvs/srv/Empty`
+
 ### device_info:
   - retrieve information about the device - serial_number, firmware_version etc.
   - Type `ros2 interface show realsense2_camera_msgs/srv/DeviceInfo` for the full list.

realsense2_camera/CMakeLists.txt

@@ -110,6 +110,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(realsense2_camera_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
@@ -245,6 +246,7 @@ set(dependencies
   rclcpp
   rclcpp_components
   realsense2_camera_msgs
+  std_srvs
   std_msgs
   sensor_msgs
   nav_msgs
@@ -318,6 +320,7 @@ if(BUILD_TESTING)
           $<INSTALL_INTERFACE:include>
        )
        ament_target_dependencies(${_test_name}
+          std_srvs
           std_msgs
        )
        #target_link_libraries(${_test_name} name_of_local_library)

realsense2_camera/include/base_realsense_node.h

@@ -27,6 +27,7 @@
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <diagnostic_updater/publisher.hpp>
+#include <std_srvs/srv/empty.hpp>
 #include "realsense2_camera_msgs/msg/imu_info.hpp"
 #include "realsense2_camera_msgs/msg/extrinsics.hpp"
 #include "realsense2_camera_msgs/msg/metadata.hpp"
@@ -155,6 +156,7 @@ namespace realsense2_camera
         std::string _camera_name;
         std::vector<rs2_option> _monitor_options;
         rclcpp::Logger _logger;
+        rclcpp::Service<std_srvs::srv::Empty>::SharedPtr _reset_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::DeviceInfo>::SharedPtr _device_info_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::CalibConfigRead>::SharedPtr _calib_config_read_srv;
         rclcpp::Service<realsense2_camera_msgs::srv::CalibConfigWrite>::SharedPtr _calib_config_write_srv;
@@ -166,6 +168,9 @@ namespace realsense2_camera
         void restartStaticTransformBroadcaster();
         void publishExtrinsicsTopic(const stream_index_pair& sip, const rs2_extrinsics& ex);
         void calcAndAppendTransformMsgs(const rs2::stream_profile& profile, const rs2::stream_profile& base_profile);
+
+        void handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req,
+                                const std_srvs::srv::Empty::Response::SharedPtr res);
         void getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req,
                                  realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res);
         void CalibConfigReadService(const realsense2_camera_msgs::srv::CalibConfigRead::Request::SharedPtr req,

realsense2_camera/package.xml

@@ -23,6 +23,7 @@
   <depend>realsense2_camera_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>std_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>tf2</depend>

realsense2_camera/src/rs_node_setup.cpp

@@ -499,6 +499,12 @@ void BaseRealSenseNode::publishServices()
 {
     // adding "~/" to the service name will add node namespace and node name to the service
     // see "Private Namespace Substitution Character" section on https://design.ros2.org/articles/topic_and_service_names.html
+    _reset_srv = _node.create_service<std_srvs::srv::Empty>(
+            "~/hw_reset",
+            [&](const std_srvs::srv::Empty::Request::SharedPtr req,
+                        std_srvs::srv::Empty::Response::SharedPtr res)
+                        {handleHWReset(req, res);});
+
     _device_info_srv = _node.create_service<realsense2_camera_msgs::srv::DeviceInfo>(
             "~/device_info",
             [&](const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr req,
@@ -535,6 +541,32 @@ void BaseRealSenseNode::publishActions()
 
 }
 
+void BaseRealSenseNode::handleHWReset(const std_srvs::srv::Empty::Request::SharedPtr req,
+                                const std_srvs::srv::Empty::Response::SharedPtr res)
+{
+    (void)req;
+    (void)res;
+    ROS_INFO_STREAM("Reset requested through service call");
+    if (_dev)
+    {
+        try
+        {
+            for(auto&& sensor : _available_ros_sensors)
+            {
+                std::string module_name(rs2_to_ros(sensor->get_info(RS2_CAMERA_INFO_NAME)));
+                ROS_INFO_STREAM("Stopping Sensor: " << module_name);
+                sensor->stop();
+            }
+            ROS_INFO("Resetting device...");
+            _dev.hardware_reset();
+        }
+        catch(const std::exception& ex)
+        {
+            ROS_WARN_STREAM("An exception has been thrown: " << __FILE__ << ":" << __LINE__ << ":" << ex.what());
+        }
+    }
+}
+
 void BaseRealSenseNode::getDeviceInfo(const realsense2_camera_msgs::srv::DeviceInfo::Request::SharedPtr,
                                             realsense2_camera_msgs::srv::DeviceInfo::Response::SharedPtr res)
 {

realsense2_camera/test/live_camera/test_d455_basic_tests.py

@@ -156,3 +156,76 @@ def test_D455_Seq_ID_update(self,launch_descr_with_parameters):
             pytest_rs_utils.kill_realsense2_camera_node()
             self.shutdown()
 
+
+
+test_params_reset_device = {
+    'camera_name': 'D455',
+    'device_type': 'D455',
+    'rgb_camera.color_profile': '640x480x30',
+    }
+'''
+This test was implemented as a template to set the parameters and run the test.
+This directory is not added to the CMakeLists.txt so as to avoid the colcon failure in the
+machines that don't have the D455 connected.
+1. Only a subset of parameter types are implemented in py_rs_utils, it has to be extended for others
+2. After setting the param, rclpy.spin_once may be needed.Test passes even without this though.
+'''
+@pytest.mark.d455
+@pytest.mark.parametrize("launch_descr_with_parameters", [test_params_reset_device],indirect=True)
+@pytest.mark.launch(fixture=launch_descr_with_parameters)
+class TestD455_reset_device(pytest_rs_utils.RsTestBaseClass):
+    def test_D455_Reset_Device(self,launch_descr_with_parameters):
+        params = launch_descr_with_parameters[1]
+        if pytest_live_camera_utils.check_if_camera_connected(params['device_type']) == False:
+            print("Device not found? : " + params['device_type'])
+            assert False
+            return
+
+        themes = [
+        {'topic':get_node_heirarchy(params)+'/color/image_raw',
+         'msg_type':msg_Image,
+         'expected_data_chunks':1,
+         'width':640,
+         'height':480,
+         #'data':data
+        }
+        ]
+        try:
+            ''' 
+            initialize, run and check the data 
+            '''
+            print("Starting camera test...")
+            self.init_test("RsTest"+params['camera_name'])
+            self.wait_for_node(params['camera_name'])
+            self.create_service_client_ifs(get_node_heirarchy(params))
+            self.spin_for_time(0.5)
+            assert self.set_bool_param('enable_color', False)
+            self.spin_for_time(0.5)
+            assert self.set_string_param('rgb_camera.color_profile', '640x480x30')
+            self.spin_for_time(0.5)
+            assert self.set_bool_param('enable_color', True)
+            self.spin_for_time(0.5)
+            ret = self.run_test(themes)
+            assert ret[0], ret[1]
+            assert self.process_data(themes)
+            self.set_string_param('rgb_camera.color_profile', '1280x800x5')
+            self.set_bool_param('enable_color', True)
+            themes[0]['width'] = 1280
+            themes[0]['height'] = 800
+
+            ret = self.run_test(themes)
+            assert ret[0], ret[1]
+            assert self.process_data(themes)
+
+            self.reset_device()
+
+            themes[0]['width'] = int(params['rgb_camera.color_profile'].split('x')[0])
+            themes[0]['height'] = int(params['rgb_camera.color_profile'].split('x')[1])
+            ret = self.run_test(themes)
+            assert ret[0], ret[1]
+            assert self.process_data(themes)
+
+        finally:
+            #this step is important because the test will fail next time
+            pytest_rs_utils.kill_realsense2_camera_node()
+            self.shutdown()

realsense2_camera/test/utils/pytest_rs_utils.py

@@ -46,6 +46,7 @@
 from rcl_interfaces.msg import Parameter
 from rcl_interfaces.msg import ParameterValue
 from rcl_interfaces.srv import SetParameters, GetParameters, ListParameters
+from std_srvs.srv import Empty
 '''
 humble doesn't have the SetParametersResult and SetParameters_Response imported using 
 __init__.py. The below two lines can be used for iron and hopefully succeeding ROS2 versions
@@ -784,18 +785,29 @@ def create_subscription(self, msg_type, topic, data_type, store_raw_data=False,
         else:
             self.node.reset_data(topic)
 
-
-
     def create_service_client_ifs(self, camera_name):
         self.set_param_if = self.node.create_client(SetParameters, camera_name + '/set_parameters')
         self.get_param_if = self.node.create_client(GetParameters, camera_name + '/get_parameters')
         self.get_device_info = self.node.create_client(DeviceInfo, camera_name + '/device_info')
+        self.reset_if = self.node.create_client(Empty, camera_name + '/hw_reset')
         while not self.get_param_if.wait_for_service(timeout_sec=1.0):
             print('service not available, waiting again...') 
         while not self.set_param_if.wait_for_service(timeout_sec=1.0):
             print('service not available, waiting again...') 
         while not self.get_device_info.wait_for_service(timeout_sec=1.0):
             print('service not available, waiting again...')
+        while not self.reset_if.wait_for_service(timeout_sec=1.0):
+            print('hw_reset service not available, waiting again...')
+
+    def reset_device(self):
+        req = Empty.Request()
+        future = self.reset_if.call_async(req)
+        while rclpy.ok():
+            rclpy.spin_once(self.node)
+            if future.done():
+                return True
+        return False
+
 
     def send_param(self, req):
         future = self.set_param_if.call_async(req)