diff --git a/README.md b/README.md index c40321bc4..49f4a1c47 100644 --- a/README.md +++ b/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. diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 439129777..3453a665d 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/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) $ ) ament_target_dependencies(${_test_name} + std_srvs std_msgs ) #target_link_libraries(${_test_name} name_of_local_library) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index c1da386cf..e6b4287a8 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -27,6 +27,7 @@ #include #include +#include #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 _monitor_options; rclcpp::Logger _logger; + rclcpp::Service::SharedPtr _reset_srv; rclcpp::Service::SharedPtr _device_info_srv; rclcpp::Service::SharedPtr _calib_config_read_srv; rclcpp::Service::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, diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index d2758d223..d4d53ae3e 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -23,6 +23,7 @@ realsense2_camera_msgs sensor_msgs geometry_msgs + std_srvs std_msgs nav_msgs tf2 diff --git a/realsense2_camera/src/rs_node_setup.cpp b/realsense2_camera/src/rs_node_setup.cpp index 2f9d9bc1d..d95c8b52f 100755 --- a/realsense2_camera/src/rs_node_setup.cpp +++ b/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( + "~/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( "~/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) { diff --git a/realsense2_camera/test/live_camera/test_d455_basic_tests.py b/realsense2_camera/test/live_camera/test_d455_basic_tests.py index 687b4949e..b008e3d34 100644 --- a/realsense2_camera/test/live_camera/test_d455_basic_tests.py +++ b/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() diff --git a/realsense2_camera/test/utils/pytest_rs_utils.py b/realsense2_camera/test/utils/pytest_rs_utils.py index 75cfb78ec..4129ae66c 100644 --- a/realsense2_camera/test/utils/pytest_rs_utils.py +++ b/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)