Added hw_reset service call to reset the device

PrasRsRos · Sep 26, 2024 · 4243224 · 4243224
1 parent c786902
commit 4243224
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 0 deletions.
diff --git 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
@@ -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

diff --git a/realsense2_camera/include/base_realsense_node.h b/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,

diff --git 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<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)
 {