diff --git a/README.md b/README.md index 651b473a5..b50f43c8c 100644 --- 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 + ```
diff --git a/realsense2_camera/src/actions.cpp b/realsense2_camera/src/actions.cpp index 740c7391b..4d65db97b 100644 --- a/realsense2_camera/src/actions.cpp +++ b/realsense2_camera/src/actions.cpp @@ -89,9 +89,15 @@ void BaseRealSenseNode::TriggeredCalibrationExecute(const std::shared_ptr(); 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_ptrcalibration = 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 index 451969080..a4884cf17 100644 --- 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 ---