diff --git a/README.md b/README.md index fac061f..e1c8576 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,7 @@ are using Zivid SDK 2.14 or newer. [**Services**](#services) | [**Topics**](#topics) | [**Samples**](#samples) | +[**RViz Plugin**](#rviz-plugin) | [**FAQ**](#frequently-asked-questions) --- @@ -277,6 +278,72 @@ service call. If the camera is not in `Connected` state the driver will attempt the camera when it detects that the camera is available. This can happen if the camera is power-cycled or the USB/Ethernet cable is unplugged and then replugged. +### infield_correction/read +[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) + +Returns the state of the [infield correction](https://support.zivid.com/en/latest/academy/camera/infield-correction.html) of the camera. + +### infield_correction/verify +[zivid_interfaces/srv/InfieldCorrectionVerify.srv](./zivid_interfaces/srv/InfieldCorrectionVerify.srv) + +Verifies the current camera trueness based on a single capture. The purpose of this service is to allow quick assessment +of the quality of the infield correction on a camera, or the need for one if none exists already. + +Returns an indication of the dimension trueness at the location where the input data was captured. If the returned +assessment indicates a trueness error that is above the threshold for your application, consider using +[infield_correction/compute_and_write](#infield_correctioncompute_and_write) and related services to update the +correction for the camera. + +### infield_correction/reset +[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) + +Resets the infield correction on the camera to factory settings. + +### infield_correction/start +[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) + +Prepares for infield correction, clears any infield correction captures gathered so far in the `zivid_camera` node. + +### infield_correction/capture +[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) + +Take a capture to be used for infield correction. Please point the camera at a Zivid infield calibration object. It is +recommended to cover several distances, with one or more captures at each distance. Successful captures are stored in +the `zivid_camera` node. The capture data set is cleared if the node is stopped, or after a call to the service +[infield_correction/start](#infield_correctionstart). + +After sufficiently number of captures, proceed by calling the service [infield_correction/compute](#infield_correctioncompute) to compute the camera correction based on the captures gathered so far. To compute the correction and write it to camera, proceed by calling the service [infield_correction/compute_and_write](#infield_correctioncompute_and_write). + +### infield_correction/compute +[zivid_interfaces/srv/InfieldCorrectionCompute.srv](./zivid_interfaces/srv/InfieldCorrectionCompute.srv) + +Calculates the new infield correction based the captured data gathered so far through the service +[infield_correction/capture](#infield_correctioncapture). + +The quantity and range of data is up to the user, but generally a larger dataset will yield a more accurate and reliable +correction. If all measurements were taken at approximately the same distance, the resulting correction will mainly be +valid at those distances. If several measurements were taken at significantly different distances, the resulting +correction will likely be more suitable for extrapolation to distances beyond where the dataset was collected. + +The service returns information regarding the proposed working range and the accuracy that can be expected within the +working range, if the correction is written to the camera. The correction may be written to the camera using +[infield_correction/compute_and_write](#infield_correctioncompute_and_write). + +### infield_correction/compute_and_write +[zivid_interfaces/srv/InfieldCorrectionCompute.srv](./zivid_interfaces/srv/InfieldCorrectionCompute.srv) + +Calculates the new infield correction based the captured data gathered so far through the service +[infield_correction/capture](#infield_correctioncapture), and writes the result to the camera. If the write operation +is successful, the infield correction capture data is cleared. + +Please see the [infield_correction/compute](#infield_correctioncompute) service for more information on the computed +correction. + +### infield_correction/remove_last_capture +[std_srvs/srv/Trigger](https://docs.ros2.org/latest/api/std_srvs/srv/Trigger.html) + +Removes the last infield correction capture gathered in the `zivid_camera` node. + ## Topics The Zivid ROS driver provides several topics providing 3D, color, SNR and camera calibration @@ -529,6 +596,50 @@ ros2 run tf2_ros static_transform_publisher --x 1 --y 2 --z 3 --yaw 1.57 --pitch Note that the robot pose and associated transform should be unique between each capture. +### Sample Infield Correction + +This sample shows how to invoke the various [infield_correction/...](#infield_correctionread) services to perform infield correction on Zivid cameras. + +Source code: [C++](./zivid_samples/src/sample_infield_correction.cpp) [Python](./zivid_samples/scripts/sample_infield_correction.py) + +```bash +ros2 launch zivid_samples sample.launch sample:=sample_infield_correction_cpp operation:= +ros2 launch zivid_samples sample.launch sample:=sample_infield_correction.py operation:= +``` +Using ros2 run (when `zivid_camera` node is already running): +```bash +ros2 run zivid_samples sample_infield_correction_cpp --ros-args -p operation:= +ros2 run zivid_samples sample_infield_correction.py --ros-args -p operation:= +``` + +Where the `` argument should be one of the following operations. + +- `verify`: Verify camera correction quality based on a single capture using the [`infield_correction/verify`](#infield_correctionverify) service. +- `correct`: Calculate in-field correction based on a series of captures at different distances. Demonstrates the use of [`infield_correction/start`](#infield_correctionstart), [`infield_correction/capture`](#infield_correctioncapture), and [`infield_correction/compute`](#infield_correctioncompute) services. Begins by preparing the camera node for infield correction captures, then the sample gathers a fixed number of captures at a fixed duration between captures. The correction result is computed after every capture. +- `correct_and_write`: Same as `correct`, but additionally writes the correction results to the camera. Demonstrates the [`infield_correction/compute_and_write`](#infield_correctioncompute_and_write) service. +- `read`: Get information about the correction currently on the connected camera using the [`infield_correction/read`](#infield_correctionread) service. +- `reset`: Reset correction on connected camera to factory settings using the [`infield_correction/reset`](#infield_correctionreset) service. + +Please see the [infield correction documentation](https://support.zivid.com/en/latest/academy/camera/infield-correction.html) for prerequisites and guidelines on how to perform the correction. + +For a more interactive experience, we recommend using the [infield correction panel](#infield-correction-panel) from the Zivid RViz plugin. + +The typical procedure for performing a new infield correction is: + +1. `start`: Prepare for infield correction, clears any existing infield correction captures. +2. `capture`: Take multiple captures from different angles and distances in accordance with the typical operating conditions of the camera. +3. `compute`: Check the computed correction results and the estimated errors, verify that they give satisfactory results. +4. `compute_and_write`: Compute the correction and write the results to camera. + +The `zivid_camera` node persists the infield correction dataset as long as it is running. To start the infield correction captures over again, use the `start` operation which clears all infield correction captures previously gathered. The `remove_last_capture` can be used to remove just the last capture. + +## RViz Plugin + +### Infield Correction Panel + +The Zivid RViz plugin provides a panel to interactively perform infield correction with a Zivid camera. + +Please see the [infield correction documentation](https://support.zivid.com/en/latest/academy/camera/infield-correction.html) for prerequisites and guidelines on how to perform the correction. ## Frequently Asked Questions diff --git a/zivid_camera/include/zivid_camera/zivid_camera.hpp b/zivid_camera/include/zivid_camera/zivid_camera.hpp index 8c7bd9a..d340267 100644 --- a/zivid_camera/include/zivid_camera/zivid_camera.hpp +++ b/zivid_camera/include/zivid_camera/zivid_camera.hpp @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include namespace Zivid @@ -66,6 +68,7 @@ enum class CameraStatus }; template class CaptureSettingsController; +struct InfieldCorrectionState; class ZividCamera : public rclcpp::Node { @@ -109,6 +112,40 @@ class ZividCamera : public rclcpp::Node const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response); + + void infieldCorrectionRead( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void infieldCorrectionReset( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void infieldCorrectionVerify( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void infieldCorrectionRemoveLastCapture( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void infieldCorrectionStart( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void infieldCorrectionCapture( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void infieldCorrectionCompute( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void infieldCorrectionComputeAndWrite( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + void publishFrame(const Zivid::Frame & frame); Zivid::Frame invokeCaptureAndPublishFrame(const Zivid::Settings & settings); bool shouldPublishPointsXYZ() const; @@ -169,6 +206,19 @@ class ZividCamera : public rclcpp::Node capture_assistant_suggest_settings_service_; rclcpp::Service::SharedPtr is_connected_service_; + std::unique_ptr infield_correction_state_; + rclcpp::Service::SharedPtr infield_correction_read_; + rclcpp::Service::SharedPtr infield_correction_reset_; + rclcpp::Service::SharedPtr + infield_correction_verify_; + rclcpp::Service::SharedPtr infield_correction_remove_last_capture_; + rclcpp::Service::SharedPtr infield_correction_start_; + rclcpp::Service::SharedPtr infield_correction_capture_; + rclcpp::Service::SharedPtr + infield_correction_compute_; + rclcpp::Service::SharedPtr + infield_correction_compute_and_write_; + std::unique_ptr zivid_; CameraStatus camera_status_{CameraStatus::Idle}; std::unique_ptr> settings_controller_; diff --git a/zivid_camera/src/zivid_camera.cpp b/zivid_camera/src/zivid_camera.cpp index 3edb2ca..d98eeed 100644 --- a/zivid_camera/src/zivid_camera.cpp +++ b/zivid_camera/src/zivid_camera.cpp @@ -27,10 +27,12 @@ // POSSIBILITY OF SUCH DAMAGE. #include +#include #include #include #include #include +#include #include #include #include @@ -52,6 +54,86 @@ struct DependentFalse : std::false_type { }; +std::string infieldCorrectionDistancesToString( + const std::vector & dataset) +{ + auto zValues = std::vector(dataset.size()); + std::transform(dataset.begin(), dataset.end(), zValues.begin(), [](const auto & input) { + return input.detectionResult().centroid().z; + }); + std::sort(zValues.begin(), zValues.end()); + + std::stringstream ss; + ss << "Measured positions: ["; + for (size_t i = 0; i < zValues.size(); ++i) { + ss << std::setprecision(1) << std::fixed << zValues.at(i) << "mm"; + if (i < zValues.size() - 1) { + ss << ", "; + } + } + ss << "]."; + + if (!zValues.empty()) { + ss << " Range covered: " << zValues.back() - zValues.front() << "mm."; + } + + return ss.str(); +} + +struct InfieldCorrectionStatistics +{ + size_t numberOfCaptures; + float currentTruenessError; + float averageTruenessError; + float maximumTruenessError; +}; + +InfieldCorrectionStatistics calculateInfieldCorrectionStatistics( + const std::vector & dataset) +{ + if (dataset.empty()) { + throw std::runtime_error("Empty dataset"); + } + const auto dimensionTruenessErrors = [&] { + std::vector result; + std::transform( + dataset.begin(), dataset.end(), std::back_inserter(result), [](const auto & input) { + return Zivid::Experimental::Calibration::verifyCamera(input).localDimensionTrueness(); + }); + return result; + }(); + const float currentTruenessError = dimensionTruenessErrors.back(); + const float averageTruenessError = + std::accumulate(cbegin(dimensionTruenessErrors), cend(dimensionTruenessErrors), 0.0F) / + static_cast(size(dimensionTruenessErrors)); + const float maximumTruenessError = + *std::max_element(dimensionTruenessErrors.begin(), dimensionTruenessErrors.end()); + return InfieldCorrectionStatistics{ + dataset.size(), currentTruenessError, averageTruenessError, maximumTruenessError}; +} + +std::string infieldCorrectionEstimateToString( + const InfieldCorrectionStatistics & statistics, + const Zivid::Experimental::Calibration::AccuracyEstimate & accuracyEstimate) +{ + std::stringstream ss; + ss << std::fixed << std::setprecision(2); + ss << "Number of captures: " << statistics.numberOfCaptures << "\n"; + ss << "Camera metrics:"; + ss << "\n Current trueness error: " << 100.0F * statistics.currentTruenessError << "%."; + if (statistics.numberOfCaptures > 1) { + ss << "\n Average trueness error: " << 100.0F * statistics.averageTruenessError << "%."; + ss << "\n Maximum trueness error: " << 100.0F * statistics.maximumTruenessError << "%."; + } + ss << "\n"; + ss << "Expected post-correction metrics:"; + ss << "\n Dimension trueness error: " << 100.0F * accuracyEstimate.dimensionAccuracy() + << "% or less."; + ss << "\n Optimized workspace (depth): " << static_cast(std::round(accuracyEstimate.zMin())) + << " - " << static_cast(std::round(accuracyEstimate.zMax())) << " mm."; + return ss.str(); +} + sensor_msgs::msg::PointField createPointField( std::string name, uint32_t offset, uint8_t datatype, uint32_t count) { @@ -148,8 +230,8 @@ void runFunctionAndCatchExceptions( const std::string & operation) { try { - function(); response->success = true; + function(); } catch (const std::exception & exception) { const auto exception_message = Zivid::toString(exception); RCLCPP_ERROR_STREAM( @@ -261,6 +343,12 @@ class CaptureSettingsController mutable std::optional cached_settings_; }; +struct InfieldCorrectionState +{ + bool started = false; + std::vector dataset; +}; + ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) : rclcpp::Node{"zivid_camera", options}, zivid_{std::make_unique( @@ -427,6 +515,30 @@ ZividCamera::ZividCamera(const rclcpp::NodeOptions & options) "capture_assistant/suggest_settings", std::bind(&ZividCamera::captureAssistantSuggestSettingsServiceHandler, this, _1, _2, _3)); + infield_correction_state_ = std::make_unique(); + infield_correction_read_ = create_service( + "infield_correction/read", std::bind(&ZividCamera::infieldCorrectionRead, this, _1, _2, _3)); + infield_correction_reset_ = create_service( + "infield_correction/reset", std::bind(&ZividCamera::infieldCorrectionReset, this, _1, _2, _3)); + infield_correction_verify_ = create_service( + "infield_correction/verify", + std::bind(&ZividCamera::infieldCorrectionVerify, this, _1, _2, _3)); + infield_correction_remove_last_capture_ = create_service( + "infield_correction/remove_last_capture", + std::bind(&ZividCamera::infieldCorrectionRemoveLastCapture, this, _1, _2, _3)); + infield_correction_start_ = create_service( + "infield_correction/start", std::bind(&ZividCamera::infieldCorrectionStart, this, _1, _2, _3)); + infield_correction_capture_ = create_service( + "infield_correction/capture", + std::bind(&ZividCamera::infieldCorrectionCapture, this, _1, _2, _3)); + infield_correction_compute_ = create_service( + "infield_correction/compute", + std::bind(&ZividCamera::infieldCorrectionCompute, this, _1, _2, _3)); + infield_correction_compute_and_write_ = + create_service( + "infield_correction/compute_and_write", + std::bind(&ZividCamera::infieldCorrectionComputeAndWrite, this, _1, _2, _3)); + RCLCPP_INFO(get_logger(), "Zivid camera driver is now ready!"); } @@ -645,6 +757,197 @@ void ZividCamera::isConnectedServiceHandler( response->is_connected = camera_status_ == CameraStatus::Connected; } +void ZividCamera::infieldCorrectionRead( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { + if (Zivid::Experimental::Calibration::hasCameraCorrection(*camera_)) { + const auto timestamp = + Zivid::Experimental::Calibration::cameraCorrectionTimestamp(*camera_); + const auto time = std::chrono::system_clock::to_time_t(timestamp); + std::stringstream ss; + ss << "Timestamp of current camera correction: " + << std::put_time(std::gmtime(&time), "%FT%TZ"); + response->message = ss.str(); + } else { + response->message = "This camera has no in-field correction written to it."; + } + }, + response, get_logger(), "InfieldCorrectionRead"); +} + +void ZividCamera::infieldCorrectionReset( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { Zivid::Experimental::Calibration::resetCameraCorrection(*camera_); }, response, + get_logger(), "InfieldCorrectionReset"); +} + +void ZividCamera::infieldCorrectionVerify( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { + RCLCPP_DEBUG_STREAM(get_logger(), "Capturing calibration object"); + const auto detectionResult = Zivid::Experimental::Calibration::detectFeaturePoints(*camera_); + const auto input = Zivid::Experimental::Calibration::InfieldCorrectionInput{detectionResult}; + if (!input.valid()) { + response->message = "Invalid capture! Feedback: " + input.statusDescription(); + response->success = false; + return; + } + RCLCPP_DEBUG_STREAM(get_logger(), "Successful measurement, starting verification"); + const auto verification = Zivid::Experimental::Calibration::verifyCamera(input); + const auto centroid = detectionResult.centroid(); + std::stringstream ss; + ss << "Successful measurement at " << detectionResult.centroid() + << ". Estimated dimension trueness error at measured position: " << std::setprecision(2) + << std::fixed << 100.0F * verification.localDimensionTrueness() << "%"; + response->local_dimension_trueness = verification.localDimensionTrueness(); + response->position.x = static_cast(centroid.x); + response->position.y = static_cast(centroid.y); + response->position.z = static_cast(centroid.z); + response->message = ss.str(); + }, + response, get_logger(), "InfieldCorrectionVerify"); +} + +void ZividCamera::infieldCorrectionRemoveLastCapture( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { + if (infield_correction_state_->dataset.empty()) { + throw std::runtime_error("Infield correction dataset is empty"); + } + infield_correction_state_->dataset.pop_back(); + }, + response, get_logger(), "InfieldCorrectionRemoveLastCapture"); +} + +void ZividCamera::infieldCorrectionStart( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { + *infield_correction_state_ = {}; + infield_correction_state_->started = true; + }, + response, get_logger(), "InfieldCorrectionStart"); +} + +void ZividCamera::infieldCorrectionCapture( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { + if (!infield_correction_state_->started) { + throw std::runtime_error( + "Infield correction not started. Please call the infield correction start service before " + "capturing."); + } + const auto detectionResult = Zivid::Calibration::detectCalibrationBoard(*camera_); + const auto input = Zivid::Experimental::Calibration::InfieldCorrectionInput{detectionResult}; + if (input.valid()) { + auto & dataset = infield_correction_state_->dataset; + dataset.push_back(input); + response->message = "Valid detection. Collected " + std::to_string(dataset.size()) + + " valid measurement(s) so far. " + + infieldCorrectionDistancesToString(dataset); + } else { + response->success = false; + response->message = "Invalid detection. This measurement will not be used. Feedback: " + + input.statusDescription(); + } + }, + response, get_logger(), "InfieldCorrectionCapture"); +} + +void ZividCamera::infieldCorrectionCompute( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { + const auto & dataset = infield_correction_state_->dataset; + // Set started & number of captures before computing, so that it is reported regardless of any exceptions. + response->infield_correction_started = infield_correction_state_->started; + response->number_of_captures = static_cast(dataset.size()); + const auto correction = Zivid::Experimental::Calibration::computeCameraCorrection(dataset); + const auto accuracyEstimate = correction.accuracyEstimate(); + const auto statistics = calculateInfieldCorrectionStatistics(dataset); + response->current_trueness_error = statistics.currentTruenessError; + response->average_trueness_error = statistics.averageTruenessError; + response->maximum_trueness_error = statistics.maximumTruenessError; + response->z_min = accuracyEstimate.zMin(); + response->z_max = accuracyEstimate.zMax(); + response->dimension_accuracy = accuracyEstimate.dimensionAccuracy(); + response->message = "Camera correction computed successfully.\n" + + infieldCorrectionEstimateToString(statistics, accuracyEstimate); + }, + response, get_logger(), "InfieldCorrectionCompute"); +} + +void ZividCamera::infieldCorrectionComputeAndWrite( + const std::shared_ptr /*request_header*/, + const std::shared_ptr /*request*/, + std::shared_ptr response) +{ + RCLCPP_INFO_STREAM(get_logger(), __func__); + + runFunctionAndCatchExceptions( + [&]() { + auto & dataset = infield_correction_state_->dataset; + response->infield_correction_started = infield_correction_state_->started; + response->number_of_captures = static_cast(dataset.size()); + const auto correction = Zivid::Experimental::Calibration::computeCameraCorrection(dataset); + RCLCPP_DEBUG(get_logger(), "Writing correction to camera"); + Zivid::Experimental::Calibration::writeCameraCorrection(*camera_, correction); + const auto accuracyEstimate = correction.accuracyEstimate(); + const auto statistics = calculateInfieldCorrectionStatistics(dataset); + response->current_trueness_error = statistics.currentTruenessError; + response->average_trueness_error = statistics.averageTruenessError; + response->maximum_trueness_error = statistics.maximumTruenessError; + response->z_min = accuracyEstimate.zMin(); + response->z_max = accuracyEstimate.zMax(); + response->dimension_accuracy = accuracyEstimate.dimensionAccuracy(); + response->message = "Camera correction successfully written to camera.\n" + + infieldCorrectionEstimateToString(statistics, accuracyEstimate); + // Clear the dataset after a successful write, the captures cannot be used again. + dataset.clear(); + }, + response, get_logger(), "InfieldCorrectionComputeAndWrite"); +} + void ZividCamera::publishFrame(const Zivid::Frame & frame) { RCLCPP_INFO_STREAM(get_logger(), __func__); diff --git a/zivid_interfaces/CMakeLists.txt b/zivid_interfaces/CMakeLists.txt index 454d17e..c66a9a0 100644 --- a/zivid_interfaces/CMakeLists.txt +++ b/zivid_interfaces/CMakeLists.txt @@ -5,15 +5,19 @@ project(zivid_interfaces) find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(zivid_interfaces "srv/CaptureAndSave.srv" "srv/CaptureAssistantSuggestSettings.srv" "srv/CameraInfoModelName.srv" "srv/CameraInfoSerialNumber.srv" + "srv/InfieldCorrectionCompute.srv" + "srv/InfieldCorrectionVerify.srv" "srv/IsConnected.srv" DEPENDENCIES - builtin_interfaces) + builtin_interfaces + geometry_msgs) ament_export_dependencies(rosidl_default_runtime) -ament_package() \ No newline at end of file +ament_package() diff --git a/zivid_interfaces/package.xml b/zivid_interfaces/package.xml index 7ce6b0c..058924c 100644 --- a/zivid_interfaces/package.xml +++ b/zivid_interfaces/package.xml @@ -14,13 +14,15 @@ builtin_interfaces rosidl_default_generators + geometry_msgs builtin_interfaces rosidl_default_runtime + geometry_msgs rosidl_interface_packages ament_cmake - \ No newline at end of file + diff --git a/zivid_interfaces/srv/InfieldCorrectionCompute.srv b/zivid_interfaces/srv/InfieldCorrectionCompute.srv new file mode 100644 index 0000000..2a189df --- /dev/null +++ b/zivid_interfaces/srv/InfieldCorrectionCompute.srv @@ -0,0 +1,33 @@ +--- +bool success +string message + +# Returns true if infield correction has been started on the target node. It needs to be started before capturing and computing. +bool infield_correction_started + +# Number of successful infield calibration captures. We recommend multiple captures with different board positions. +int32 number_of_captures + +# Camera metrics +float32 current_trueness_error +float32 average_trueness_error +float32 maximum_trueness_error + +# The estimated dimension accuracy error obtained if the correction is applied. +# +# This number represents a 1-sigma (68% confidence) upper bound for dimension trueness error in the working volume +# (z=zMin() to z=zMax(), across the entire field of view). In other words, it represents the expected distribution of +# local dimension trueness measurements (see ) that can be expected if measuring +# throughout the working volume. +# +# The value is a fraction (relative trueness error). Multiply by 100 to get trueness in percent. +# +# Note that the accuracy close to where the original data was captured is likely much better than what is implied by +# this number. This number is rather an accuracy estimate for the entire extrapolated working volume. +float32 dimension_accuracy + +# The range of validity of the accuracy estimate (lower end). +float32 z_min + +# The range of validity of the accuracy estimate (upper end). +float32 z_max diff --git a/zivid_interfaces/srv/InfieldCorrectionVerify.srv b/zivid_interfaces/srv/InfieldCorrectionVerify.srv new file mode 100644 index 0000000..0621a13 --- /dev/null +++ b/zivid_interfaces/srv/InfieldCorrectionVerify.srv @@ -0,0 +1,14 @@ +--- +bool success +string message + +# The estimated local dimension trueness error. The dimension trueness represents the relative deviation between the +# measured size of the calibration object and the true size of the calibration object, including the effects of any +# in-field correction stored on the camera at the time of capture. Note that this estimate is local, i.e. only valid for +# the region of space very close to the calibration object. +# +# The value is a fraction (relative trueness error). Multiply by 100 to get trueness error in percent. +float32 local_dimension_trueness + +# The location at which the measurement was made. +geometry_msgs/Point position diff --git a/zivid_rviz_plugin/CMakeLists.txt b/zivid_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000..02f14f9 --- /dev/null +++ b/zivid_rviz_plugin/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) +project(zivid_rviz_plugin) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rviz_common REQUIRED) +find_package(std_srvs REQUIRED) +find_package(zivid_interfaces REQUIRED) + +set(CMAKE_AUTOMOC ON) +qt5_wrap_cpp(MOC_FILES + include/zivid_rviz_plugin/infield_correction.hpp +) + +add_library(zivid_rviz_plugin src/infield_correction.cpp ${MOC_FILES}) +target_include_directories(zivid_rviz_plugin PUBLIC + $ + $ +) +ament_target_dependencies(zivid_rviz_plugin + rclcpp + pluginlib + rviz_common + std_srvs + zivid_interfaces +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install(TARGETS zivid_rviz_plugin + EXPORT export_zivid_rviz_plugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/ + DESTINATION include +) +install(FILES plugins.xml + DESTINATION share/${PROJECT_NAME} +) +install(FILES "icons/classes/Zivid Infield Correction.png" + DESTINATION share/${PROJECT_NAME}/icons/classes +) +ament_export_include_directories(include) +ament_export_targets(export_zivid_rviz_plugin) +pluginlib_export_plugin_description_file(rviz_common plugins.xml) + +ament_package() diff --git a/zivid_rviz_plugin/icons/classes/Zivid Infield Correction.png b/zivid_rviz_plugin/icons/classes/Zivid Infield Correction.png new file mode 100644 index 0000000..ec22aeb Binary files /dev/null and b/zivid_rviz_plugin/icons/classes/Zivid Infield Correction.png differ diff --git a/zivid_rviz_plugin/include/zivid_rviz_plugin/infield_correction.hpp b/zivid_rviz_plugin/include/zivid_rviz_plugin/infield_correction.hpp new file mode 100644 index 0000000..b231fa6 --- /dev/null +++ b/zivid_rviz_plugin/include/zivid_rviz_plugin/infield_correction.hpp @@ -0,0 +1,61 @@ +#ifndef RVIZ_INFIELD_CORRECTION_HPP +#define RVIZ_INFIELD_CORRECTION_HPP + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace zivid_rviz_plugin +{ + +class InfieldCorrection : public rviz_common::Panel +{ + Q_OBJECT +public: + explicit InfieldCorrection(QWidget * parent = nullptr); + ~InfieldCorrection() override; + + void onInitialize() override; + +private Q_SLOTS: + void onCaptureClicked(); + void onComputeAndWriteClicked(); + void onRemoveLastCaptureClicked(); + void onStartClicked(); + void onRestartCapturesClicked(); + +private: + void setCorrectionStarted(bool started); + void sendStartRequest(const std::string & tool_name); + void sendComputeRequestAndShowResult( + const std::string & description_preamble = {}, bool silence_zero_captures_error = false, + bool write_to_camera = false); + bool updateVisibilityAndEnabledStates(); + + std::shared_ptr node_ptr_; + + rclcpp::Client::SharedPtr capture_client_; + rclcpp::Client::SharedPtr remove_last_capture_client_; + rclcpp::Client::SharedPtr start_client_; + rclcpp::Client::SharedPtr compute_client_; + rclcpp::Client::SharedPtr + compute_and_write_client_; + + rclcpp::TimerBase::SharedPtr camera_availability_timer_; + + bool correction_started = false; + QLabel * waiting_for_camera_label_ = nullptr; + QLabel * description_ = nullptr; + QVBoxLayout * layout_ = nullptr; + QPushButton * start_button_ = nullptr; + QPushButton * capture_button_ = nullptr; +}; + +} // namespace zivid_rviz_plugin + +#endif //RVIZ_INFIELD_CORRECTION_HPP diff --git a/zivid_rviz_plugin/package.xml b/zivid_rviz_plugin/package.xml new file mode 100644 index 0000000..b278350 --- /dev/null +++ b/zivid_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + zivid_rviz_plugin + 3.0.0 + RViz plugins for Zivid 3D cameras. + Zivid + BSD3 + + ament_cmake + + rclcpp + rviz_common + std_srvs + zivid_interfaces + pluginlib + + ament_lint_auto + ament_cmake_clang_format + ament_cmake_clang_tidy + ament_cmake_cppcheck + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_xmllint + + + ament_cmake + + diff --git a/zivid_rviz_plugin/plugins.xml b/zivid_rviz_plugin/plugins.xml new file mode 100644 index 0000000..4cdb6da --- /dev/null +++ b/zivid_rviz_plugin/plugins.xml @@ -0,0 +1,5 @@ + + + Infield correction for Zivid 3D cameras. + + diff --git a/zivid_rviz_plugin/src/infield_correction.cpp b/zivid_rviz_plugin/src/infield_correction.cpp new file mode 100644 index 0000000..a74da71 --- /dev/null +++ b/zivid_rviz_plugin/src/infield_correction.cpp @@ -0,0 +1,255 @@ +#include +#include +#include +#include +#include + +namespace zivid_rviz_plugin +{ +InfieldCorrection::InfieldCorrection(QWidget * parent) : rviz_common::Panel(parent) +{ + constexpr int max_content_width = 400; + + layout_ = new QVBoxLayout; + + waiting_for_camera_label_ = new QLabel("Waiting for the Zivid camera node to become available."); + waiting_for_camera_label_->setVisible(false); + waiting_for_camera_label_->setWordWrap(true); + waiting_for_camera_label_->setMaximumWidth(max_content_width); + layout_->addWidget(waiting_for_camera_label_); + + start_button_ = new QPushButton("Start infield correction"); + start_button_->setMaximumWidth(max_content_width); + layout_->addWidget(start_button_); + connect(start_button_, &QPushButton::clicked, this, &InfieldCorrection::onStartClicked); + + capture_button_ = new QPushButton("Capture && Measure"); + capture_button_->setMaximumWidth(max_content_width); + capture_button_->setVisible(false); + layout_->addWidget(capture_button_); + connect(capture_button_, &QPushButton::clicked, this, &InfieldCorrection::onCaptureClicked); + + description_ = new QLabel; + description_->setWordWrap(true); + description_->setMaximumWidth(max_content_width); + layout_->addStretch(1); + layout_->addWidget(description_); + layout_->addStretch(1); + + auto compute_and_write_button = new QPushButton("Save correction to camera"); + compute_and_write_button->setMaximumWidth(max_content_width); + layout_->addWidget(compute_and_write_button); + connect( + compute_and_write_button, &QPushButton::clicked, this, + &InfieldCorrection::onComputeAndWriteClicked); + + auto split_button_container = new QWidget; + split_button_container->setMaximumWidth(max_content_width); + layout_->addWidget(split_button_container); + + auto split_button = new QHBoxLayout(split_button_container); + split_button->setContentsMargins(0, 0, 0, 0); + + auto remove_last_capture_button = new QPushButton("Remove last capture"); + split_button->addWidget(remove_last_capture_button); + connect( + remove_last_capture_button, &QPushButton::clicked, this, + &InfieldCorrection::onRemoveLastCaptureClicked); + + auto restart_captures_button = new QPushButton("Restart captures"); + split_button->addWidget(restart_captures_button); + connect( + restart_captures_button, &QPushButton::clicked, this, + &InfieldCorrection::onRestartCapturesClicked); + + setLayout(layout_); +} + +InfieldCorrection::~InfieldCorrection() = default; + +void InfieldCorrection::onInitialize() +{ + node_ptr_ = getDisplayContext()->getRosNodeAbstraction().lock(); + + rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node(); + + capture_client_ = node->create_client("infield_correction/capture"); + remove_last_capture_client_ = + node->create_client("infield_correction/remove_last_capture"); + start_client_ = node->create_client("infield_correction/start"); + compute_client_ = node->create_client( + "infield_correction/compute"); + compute_and_write_client_ = node->create_client( + "infield_correction/compute_and_write"); + + updateVisibilityAndEnabledStates(); + + camera_availability_timer_ = + node_ptr_->get_raw_node()->create_wall_timer(std::chrono::seconds(3), [this]() { + const bool is_available_before = !waiting_for_camera_label_->isVisible(); + const bool is_available_after = updateVisibilityAndEnabledStates(); + if (!is_available_before && is_available_after) { + sendComputeRequestAndShowResult({}, true); + } + }); + + sendComputeRequestAndShowResult({}, true); + + RCLCPP_INFO(node_ptr_->get_raw_node()->get_logger(), "Infield correction panel initialized"); +} + +void InfieldCorrection::onCaptureClicked() +{ + if (!updateVisibilityAndEnabledStates()) { + return; + } + RCLCPP_INFO( + node_ptr_->get_raw_node()->get_logger(), "Infield correction: Sending %s request.", + capture_client_->get_service_name()); + capture_client_->async_send_request( + std::make_shared(), + [this](rclcpp::Client::SharedFuture future) { + const auto & response = future.get(); + const std::string message = + (response->success ? "Infield correction capture succeeded." + : "Infield correction capture failed: " + response->message); + sendComputeRequestAndShowResult(message); + RCLCPP_INFO( + node_ptr_->get_raw_node()->get_logger(), "Infield correction: %s", message.c_str()); + }); +} + +void InfieldCorrection::onComputeAndWriteClicked() +{ + sendComputeRequestAndShowResult("Writing computed infield correction to camera.", false, true); +} + +void InfieldCorrection::onRemoveLastCaptureClicked() +{ + if (!updateVisibilityAndEnabledStates()) { + return; + } + RCLCPP_INFO( + node_ptr_->get_raw_node()->get_logger(), "Infield correction: Sending %s request.", + remove_last_capture_client_->get_service_name()); + remove_last_capture_client_->async_send_request( + std::make_shared(), + [this](rclcpp::Client::SharedFuture future) { + const auto & response = future.get(); + std::string message; + if (response->success) { + message = "Last capture successfully removed."; + } else { + message = "Failed to remove last capture: " + response->message; + } + sendComputeRequestAndShowResult(message, true); + RCLCPP_INFO( + node_ptr_->get_raw_node()->get_logger(), "Infield correction: %s", message.c_str()); + }); +} + +void InfieldCorrection::onStartClicked() +{ + if (!updateVisibilityAndEnabledStates()) { + return; + } + sendStartRequest("start"); +} + +void InfieldCorrection::onRestartCapturesClicked() +{ + if (!updateVisibilityAndEnabledStates()) { + return; + } + sendStartRequest("restart"); +} + +void InfieldCorrection::setCorrectionStarted(bool started) +{ + if (started != correction_started) { + start_button_->setVisible(!started); + capture_button_->setVisible(started); + correction_started = started; + updateVisibilityAndEnabledStates(); + } +} + +void InfieldCorrection::sendStartRequest(const std::string & tool_name) +{ + RCLCPP_INFO( + node_ptr_->get_raw_node()->get_logger(), "Infield correction: Sending %s request.", + start_client_->get_service_name()); + start_client_->async_send_request( + std::make_shared(), + [this, tool_name](rclcpp::Client::SharedFuture future) { + const auto & response = future.get(); + std::string message; + if (response->success) { + message = "Infield correction tool " + tool_name + "ed, any existing captures erased."; + } else { + message = "Failed to " + tool_name + " infield correction tool: " + response->message; + } + sendComputeRequestAndShowResult(message, true); + RCLCPP_INFO( + node_ptr_->get_raw_node()->get_logger(), "Infield correction: %s", message.c_str()); + }); +} + +void InfieldCorrection::sendComputeRequestAndShowResult( + const std::string & description_preamble, bool silence_zero_captures_error, bool write_to_camera) +{ + auto & client = (write_to_camera ? compute_and_write_client_ : compute_client_); + RCLCPP_INFO( + node_ptr_->get_raw_node()->get_logger(), "Infield correction: Sending %s request.", + client->get_service_name()); + client->async_send_request( + std::make_shared(), + [this, description_preamble, silence_zero_captures_error]( + rclcpp::Client::SharedFuture future) { + std::string message; + if (!description_preamble.empty()) { + message = description_preamble + "\n\n"; + } + const auto & response = future.get(); + setCorrectionStarted(response->infield_correction_started); + + if (silence_zero_captures_error && !response->success && response->number_of_captures == 0) { + if (correction_started) { + message += + "No captures done. We recommend multiple captures with different board positions."; + } else { + message += "Infield correction tool ready to be started."; + } + } else if (!response->success) { + message += "Failed to compute infield correction: " + response->message; + } else { + message += response->message; + } + + description_->setText(QString(message.c_str())); + }); +} + +bool InfieldCorrection::updateVisibilityAndEnabledStates() +{ + const bool service_available = (start_client_ && start_client_->service_is_ready()); + if (service_available == waiting_for_camera_label_->isVisible()) { + waiting_for_camera_label_->setVisible(!service_available); + } + + for (auto * child : children()) { + auto * widget = qobject_cast(child); + if (widget && widget != waiting_for_camera_label_) { + const bool enable_widget = + (service_available && + (correction_started || widget == start_button_ || widget == description_)); + widget->setEnabled(enable_widget); + } + } + + return service_available; +} +} // namespace zivid_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(zivid_rviz_plugin::InfieldCorrection, rviz_common::Panel) diff --git a/zivid_samples/CMakeLists.txt b/zivid_samples/CMakeLists.txt index 479a41e..e322a7a 100644 --- a/zivid_samples/CMakeLists.txt +++ b/zivid_samples/CMakeLists.txt @@ -40,6 +40,7 @@ zivid_add_cpp_sample(sample_capture_2d DEPENDENCIES rclcpp sensor_msgs std_srvs) zivid_add_cpp_sample(sample_capture_and_save DEPENDENCIES rclcpp zivid_interfaces) zivid_add_cpp_sample(sample_capture_assistant DEPENDENCIES rclcpp zivid_interfaces sensor_msgs std_srvs) zivid_add_cpp_sample(sample_capture_with_settings_from_file DEPENDENCIES rclcpp sensor_msgs std_srvs ament_index_cpp) +zivid_add_cpp_sample(sample_infield_correction DEPENDENCIES rclcpp std_srvs zivid_interfaces) find_package(Zivid 2.13.0 COMPONENTS Core) if(TARGET Zivid::Core) diff --git a/zivid_samples/launch/sample.launch b/zivid_samples/launch/sample.launch index 783d6b6..c99c600 100644 --- a/zivid_samples/launch/sample.launch +++ b/zivid_samples/launch/sample.launch @@ -1,6 +1,7 @@ + @@ -8,6 +9,7 @@ + diff --git a/zivid_samples/src/sample_hand_eye_calibration.cpp b/zivid_samples/src/sample_hand_eye_calibration.cpp index 2f58a7a..27a96a4 100644 --- a/zivid_samples/src/sample_hand_eye_calibration.cpp +++ b/zivid_samples/src/sample_hand_eye_calibration.cpp @@ -183,15 +183,15 @@ class HandEyeCalibrationNode : public rclcpp::Node m_capture_and_save_client{create_client( "capture_and_save", default_rclcpp_services_qos, m_callback_group)}, m_capture_service{create_service( - "hand_eye_capture", + std::string(get_name()) + "/capture", std::bind( - &HandEyeCalibrationNode::hand_eye_capture_callback, this, std::placeholders::_1, + &HandEyeCalibrationNode::capture_callback, this, std::placeholders::_1, std::placeholders::_2), default_rclcpp_services_qos, m_callback_group)}, m_calibrate_service{create_service( - "hand_eye_calibrate", + std::string(get_name()) + "/calibrate", std::bind( - &HandEyeCalibrationNode::hand_eye_calibrate_callback, this, std::placeholders::_1, + &HandEyeCalibrationNode::calibrate_callback, this, std::placeholders::_1, std::placeholders::_2), default_rclcpp_services_qos, m_callback_group)} { @@ -240,7 +240,7 @@ class HandEyeCalibrationNode : public rclcpp::Node std::string message; }; - void hand_eye_capture_callback( + void capture_callback( const std::shared_ptr & /*request*/, const std::shared_ptr & response) { @@ -271,7 +271,7 @@ class HandEyeCalibrationNode : public rclcpp::Node index, m_capture_service->get_service_name(), m_calibrate_service->get_service_name()); } - void hand_eye_calibrate_callback( + void calibrate_callback( const std::shared_ptr & /*request*/, const std::shared_ptr & response) const { diff --git a/zivid_samples/src/sample_infield_correction.cpp b/zivid_samples/src/sample_infield_correction.cpp new file mode 100644 index 0000000..a0d2639 --- /dev/null +++ b/zivid_samples/src/sample_infield_correction.cpp @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include + +static const auto read_only_parameter = + rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); + +void fatal_error(const rclcpp::Logger & logger, const std::string & message) +{ + RCLCPP_ERROR_STREAM(logger, message); + throw std::runtime_error(message); +} + +template +std::string join_list_to_string(const List & list, ToStringFunc && to_string_func) +{ + return list.empty() + ? std::string() + : std::accumulate( + std::next(std::begin(list)), std::end(list), to_string_func(*std::begin(list)), + [&](const std::string & str, const auto & entry) { + return str + ", " + to_string_func(entry); + }); +} + +template +Enum get_parameter_enum( + rclcpp::Node & node, const std::string & name, const std::map & name_value_map) +{ + const auto value = node.declare_parameter(name, "", read_only_parameter); + const auto it = name_value_map.find(value); + if (it == name_value_map.end()) { + const std::string valid_values = + join_list_to_string(name_value_map, [](auto && pair) { return pair.first; }); + fatal_error( + node.get_logger(), "Invalid value for parameter '" + name + "': '" + value + + "'. Expected one of: " + valid_values + "."); + } + RCLCPP_INFO(node.get_logger(), "%s", ("Got parameter " + name + ": " + value).c_str()); + return it->second; +} + +enum class InfieldCorrectionOperation +{ + Verify, + Correct, + CorrectAndWrite, + Read, + Reset, +}; + +const std::map infield_correction_operation_map = { + {"verify", InfieldCorrectionOperation::Verify}, + {"correct", InfieldCorrectionOperation::Correct}, + {"correct_and_write", InfieldCorrectionOperation::CorrectAndWrite}, + {"read", InfieldCorrectionOperation::Read}, + {"reset", InfieldCorrectionOperation::Reset}, +}; + +std::shared_ptr> create_trigger_client( + const rclcpp::Node::SharedPtr & node, const std::string & service_name) +{ + auto client = node->create_client(service_name); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO( + node->get_logger(), "Waiting for the %s service to appear...", client->get_service_name()); + } + return client; +} + +std::shared_ptr> +create_infield_correction_compute_client( + const rclcpp::Node::SharedPtr & node, const std::string & service_name) +{ + auto client = node->create_client(service_name); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO( + node->get_logger(), "Waiting for the %s service to appear...", client->get_service_name()); + } + return client; +} + +std::shared_ptr> +create_infield_correction_verify_client(const rclcpp::Node::SharedPtr & node) +{ + auto client = node->create_client( + "infield_correction/verify"); + while (!client->wait_for_service(std::chrono::seconds(3))) { + if (!rclcpp::ok()) { + fatal_error(node->get_logger(), "Client interrupted while waiting for service to appear."); + } + RCLCPP_INFO( + node->get_logger(), "Waiting for the %s service to appear...", client->get_service_name()); + } + return client; +} + +bool request_trigger_and_print_response( + const rclcpp::Node::SharedPtr & node, + const std::shared_ptr> & client) +{ + auto future = client->async_send_request(std::make_shared()); + if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to call the infield correction compute service"); + } + auto result = future.get(); + RCLCPP_INFO(node->get_logger(), "Trigger results (%s):", client->get_service_name()); + RCLCPP_INFO(node->get_logger(), " Success: %s", result->success ? "true" : "false"); + RCLCPP_INFO( + node->get_logger(), " Message: %s", + result->message.empty() ? "" : (R"(""")" + result->message + R"(""")").c_str()); + return result->success; +} + +void request_infield_correction_verify_and_print_response( + const rclcpp::Node::SharedPtr & node, + const std::shared_ptr> & client) +{ + auto future = client->async_send_request( + std::make_shared()); + if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to call the infield correction verify service"); + } + auto result = future.get(); + RCLCPP_INFO(node->get_logger(), "Infield verification results (%s):", client->get_service_name()); + RCLCPP_INFO(node->get_logger(), " Success: %s", result->success ? "true" : "false"); + RCLCPP_INFO( + node->get_logger(), " Local dimension trueness: %g", result->local_dimension_trueness); + RCLCPP_INFO( + node->get_logger(), " Position: %g, %g, %g", result->position.x, result->position.y, + result->position.z); + RCLCPP_INFO( + node->get_logger(), " Message: %s", + result->message.empty() ? "" : (R"(""")" + result->message + R"(""")").c_str()); +} + +void request_infield_correction_compute_and_print_response( + const rclcpp::Node::SharedPtr & node, + const std::shared_ptr> & client, + const std::string & name) +{ + auto future = client->async_send_request( + std::make_shared()); + if (rclcpp::spin_until_future_complete(node, future) != rclcpp::FutureReturnCode::SUCCESS) { + fatal_error(node->get_logger(), "Failed to call the infield correction " + name + " service"); + } + auto result = future.get(); + RCLCPP_INFO( + node->get_logger(), "Infield correction compute results (%s):", client->get_service_name()); + RCLCPP_INFO(node->get_logger(), " Success: %s", result->success ? "true" : "false"); + RCLCPP_INFO(node->get_logger(), " Number of captures: %d", result->number_of_captures); + RCLCPP_INFO(node->get_logger(), " Current trueness error: %g", result->current_trueness_error); + RCLCPP_INFO(node->get_logger(), " Average trueness error: %g", result->average_trueness_error); + RCLCPP_INFO(node->get_logger(), " Maximum trueness error: %g", result->maximum_trueness_error); + RCLCPP_INFO(node->get_logger(), " Dimension accuracy: %g", result->dimension_accuracy); + RCLCPP_INFO(node->get_logger(), " Z min: %g", result->z_min); + RCLCPP_INFO(node->get_logger(), " Z max: %g", result->z_max); + RCLCPP_INFO( + node->get_logger(), " Message: %s", + result->message.empty() ? "" : (R"(""")" + result->message + R"(""")").c_str()); +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared("sample_infield_correction"); + + const InfieldCorrectionOperation operation = + get_parameter_enum(*node, "operation", infield_correction_operation_map); + + switch (operation) { + case InfieldCorrectionOperation::Verify: { + RCLCPP_INFO(node->get_logger(), "--- Starting infield correction: Verify ---"); + auto verify_client = create_infield_correction_verify_client(node); + request_infield_correction_verify_and_print_response(node, verify_client); + } break; + case InfieldCorrectionOperation::Correct: + case InfieldCorrectionOperation::CorrectAndWrite: { + RCLCPP_INFO(node->get_logger(), "--- Starting infield correction ---"); + auto start_client = create_trigger_client(node, "infield_correction/start"); + if (!request_trigger_and_print_response(node, start_client)) { + fatal_error(node->get_logger(), "Could not start infield correction, aborting."); + } + + auto capture_client = create_trigger_client(node, "infield_correction/capture"); + auto compute_client = + create_infield_correction_compute_client(node, "infield_correction/compute"); + + constexpr int wait_seconds = 5; + constexpr int num_successful_captures_target = 3; + RCLCPP_INFO_STREAM( + node->get_logger(), "--- Starting captures, will proceed until " + + std::to_string(num_successful_captures_target) + + " captures have completed with a detected calibration board ---"); + + bool sleep_before_capture = false; + for (int num_successful_captures = 0; + num_successful_captures < num_successful_captures_target;) { + if (sleep_before_capture) { + RCLCPP_INFO_STREAM( + node->get_logger(), "--- Waiting for " + std::to_string(wait_seconds) + + " seconds before taking the next capture ---"); + rclcpp::spin_until_future_complete( + node, std::promise().get_future(), std::chrono::seconds(wait_seconds)); + } else { + sleep_before_capture = true; + } + + const bool success = request_trigger_and_print_response(node, capture_client); + if (success) { + num_successful_captures += 1; + // Run compute on the gathered captures so far. This provides useful information about the + // expected effect of the correction with the current captures. + request_infield_correction_compute_and_print_response(node, compute_client, "compute"); + } + } + + RCLCPP_INFO(node->get_logger(), "--- Captures complete ---"); + + if (operation == InfieldCorrectionOperation::CorrectAndWrite) { + RCLCPP_INFO_STREAM(node->get_logger(), "--- Writing correction results to camera ---"); + auto compute_and_write_client = + create_infield_correction_compute_client(node, "infield_correction/compute_and_write"); + request_infield_correction_compute_and_print_response( + node, compute_and_write_client, "compute and write"); + } + + } break; + case InfieldCorrectionOperation::Read: { + RCLCPP_INFO(node->get_logger(), "--- Starting infield correction: Read ---"); + auto read_client = create_trigger_client(node, "infield_correction/read"); + request_trigger_and_print_response(node, read_client); + } break; + case InfieldCorrectionOperation::Reset: { + RCLCPP_INFO(node->get_logger(), "--- Starting infield correction: Reset ---"); + auto reset_client = create_trigger_client(node, "infield_correction/reset"); + request_trigger_and_print_response(node, reset_client); + } break; + default: { + fatal_error( + node->get_logger(), "Unknown operation: " + std::to_string(static_cast(operation))); + } break; + } + + RCLCPP_INFO(node->get_logger(), "Spinning node.. Press Ctrl+C to abort."); + rclcpp::spin(node); + + rclcpp::shutdown(); + return EXIT_SUCCESS; +}