PR #3153 from SamerKhshiboun: TC | Fix feedback and update readme

IntelRealSense · Jul 2, 2024 · 4e94c66 · 4e94c66
2 parents b99f251 + f53762d
commit 4e94c66
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 6 deletions.
diff --git a/README.md b/README.md
@@ -679,13 +679,15 @@ Each of the above filters have it's own parameters, following the naming convent
 
 ## Available actions
 
-### triggered_calibration
+### triggered_calibration (supported only for D500 devices)
   - Type `ros2 interface show realsense2_camera_msgs/action/TriggeredCalibration` for the full request/result/feedback fields.
     ```
     # request
     string json "calib run"  # default value
     ---
     # result
+    bool success
+    string error_msg
     string calibration
     float32 health
     ---
@@ -694,8 +696,16 @@ Each of the above filters have it's own parameters, following the naming convent
 
     ```
   - To use from command line: `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration '{json: "{calib run}"}'` or even with an empty request `ros2 action send_goal /camera/camera/triggered_calibration realsense2_camera_msgs/action/TriggeredCalibration ''` because the default behavior is already calib run.
-  - The action gives an updated feedback about the progress (%) if the client asks for feedback.
+  - The action gives an updated feedback about the progress (%) if the client asks for feedback. To do that, add `--feedback` to the end of the command.
   - If succeded, the action writes the new calibration table to the flash. It also returns the new calibration table as json string and the health as float32
+  - If failed, it will return the error message inside the result. For example:
+  ```
+  Result:
+    success: false
+    error_msg: 'TriggeredCalibrationExecute: Aborted. Error: Calibration completed but algorithm failed'
+    calibration: '{}'
+    health: 0.0
+  ```
 
 <hr>
 

diff --git a/realsense2_camera/src/actions.cpp b/realsense2_camera/src/actions.cpp
@@ -89,9 +89,15 @@ void BaseRealSenseNode::TriggeredCalibrationExecute(const std::shared_ptr<GoalHa
         rs2::auto_calibrated_device ac_dev = _dev.as<auto_calibrated_device>();
         float health = 0.f;  // output health
         int timeout_ms = 120000;  // 2 minutes timout
+
+        auto progress_callback = [&](const float progress) {
+                _progress = progress;
+                goal_handle->publish_feedback(feedback);
+        };
+
         auto ans = ac_dev.run_on_chip_calibration(goal->json,
                                                 &health,
-                                                [&](const float progress) {_progress = progress; },
+                                                progress_callback,
                                                 timeout_ms);
 
         // the new calibration is the result without the first 3 bytes
@@ -101,21 +107,27 @@ void BaseRealSenseNode::TriggeredCalibrationExecute(const std::shared_ptr<GoalHa
         {
             result->calibration = vectorToJsonString(new_calib);
             result->health = health;
+            result->success = true;
             goal_handle->succeed(result);
-            ROS_DEBUG("TriggeredCalibrationExecute: Succeded");
+            ROS_INFO("TriggeredCalibrationExecute: Succeded");
         }
         else
         {
             result->calibration = "{}";
+            result->success = false;
+            result->error_msg = "Canceled";
             goal_handle->canceled(result);
             ROS_WARN("TriggeredCalibrationExecute: Canceled");
         }
     }
-    catch(...)
+    catch(const std::runtime_error& e)
     {
         // exception must have been thrown from run_on_chip_calibration call
+        std::string error_msg = "TriggeredCalibrationExecute: Aborted. Error: " + std::string(e.what());
         result->calibration = "{}";
+        result->success = false;
+        result->error_msg = error_msg;
         goal_handle->abort(result);
-        ROS_ERROR("TriggeredCalibrationExecute: Aborted");
+        ROS_ERROR(error_msg.c_str());
     }
 }
diff --git a/realsense2_camera_msgs/action/TriggeredCalibration.action b/realsense2_camera_msgs/action/TriggeredCalibration.action
@@ -2,6 +2,8 @@
 string json "calib run"
 ---
 # result
+bool success
+string error_msg
 string calibration
 float32 health
 ---