Add service to capture and save frame

It is very valuable to be able to save the frame
as a ZDF file, or export to other formats.
This commit introduces the service capture_and_save_frame
which both captures and saves the frame. It takes a
path as an argument. See README.md#capture_and_save_frame
for more information.

In order to show how to use this service we also add a
sample, both for Python and C++.

Closes #65

README.md

@@ -301,6 +301,16 @@ visible via dynamic_reconfigure, see section [Configuration](#configuration). Wh
 service has returned you can invoke the [capture_2d](#capture) service to trigger a 2D capture
 using these settings.
 
+### capture_and_save_frame
+[zivid_camera/CaptureAndSaveFrame.srv](./zivid_camera/srv/CaptureAndSaveFrame.srv)
+
+It does exactly the same as the [capture](#capture) service in addition to saving the frame to
+a file. This service takes a path as an argument. The chosen format is detected via the path extension.
+See [Point Cloud](https://support.zivid.com/latest/reference-articles/point-cloud-structure-and-output-formats.html)
+for a list of available formats.
+
+See [Sample Capture and Save Frame](#sample-capture-and-save-frame) for code example.
+
 ### camera_info/model_name
 [zivid_camera/CameraInfoModelName.srv](./zivid_camera/srv/CameraInfoModelName.srv)
 
@@ -618,6 +628,27 @@ rosrun zivid_samples sample_capture_with_settings_from_yml_cpp
 rosrun zivid_samples sample_capture_with_settings_from_yml.py
 ```
 
+### Sample Capture and Save Frame
+
+This sample performs a single-acquisition 3D capture and saves the frame as ZDF file.
+This sample shows how to simply [enable the first default acquisition](#configuration),
+and how to invoke the [capture_and_save_frame](#capture_and_save_frame) service.
+The file is saved to `/tmp/capture_py.zdf` for the Python sample and
+`/tmp/capture_cpp.zdf` for the C++ sample.
+
+Source code: [C++](./zivid_samples/src/sample_capture_and_save_frame.cpp), [Python](./zivid_samples/scripts/sample_capture_and_save_frame.py)
+
+Using roslaunch (also launches `roscore`, `zivid_camera`, `rviz` and `rqt_reconfigure`):
+```bash
+roslaunch zivid_samples sample.launch type:=sample_capture_and_save_frame_cpp
+roslaunch zivid_samples sample.launch type:=sample_capture_and_save_frame.py
+```
+Using rosrun (when `roscore` and `zivid_camera` are running):
+```bash
+rosrun zivid_samples sample_capture_and_save_frame_cpp
+rosrun zivid_samples sample_capture_and_save_frame.py
+```
+
 ## Frequently Asked Questions
 
 ### How to visualize the output from the camera in rviz

zivid_camera/CMakeLists.txt

@@ -84,6 +84,7 @@ add_service_files(
   srv
   FILES
   Capture.srv
+  CaptureAndSaveFrame.srv
   Capture2D.srv
   CaptureAssistantSuggestSettings.srv
   LoadSettingsFromFile.srv

zivid_camera/include/auto_generated_include_wrapper.h

@@ -12,6 +12,7 @@
 #include <zivid_camera/Settings2DAcquisitionConfig.h>
 #include <zivid_camera/Settings2DConfig.h>
 #include <zivid_camera/Capture.h>
+#include <zivid_camera/CaptureAndSaveFrame.h>
 #include <zivid_camera/Capture2D.h>
 #include <zivid_camera/CaptureAssistantSuggestSettings.h>
 #include <zivid_camera/CameraInfoModelName.h>

zivid_camera/include/zivid_camera.h

@@ -43,14 +43,16 @@ class ZividCamera
   bool cameraInfoSerialNumberServiceHandler(CameraInfoSerialNumber::Request& req,
                                             CameraInfoSerialNumber::Response& res);
   bool captureServiceHandler(Capture::Request& req, Capture::Response& res);
+  bool captureAndSaveFrameServiceHandler(CaptureAndSaveFrame::Request& req, CaptureAndSaveFrame::Response& res);
   bool capture2DServiceHandler(Capture2D::Request& req, Capture2D::Response& res);
   bool captureAssistantSuggestSettingsServiceHandler(CaptureAssistantSuggestSettings::Request& req,
                                                      CaptureAssistantSuggestSettings::Response& res);
   bool loadSettingsFromFileServiceHandler(LoadSettingsFromFile::Request& req, LoadSettingsFromFile::Response&);
   bool loadSettings2DFromFileServiceHandler(LoadSettings2DFromFile::Request& req, LoadSettings2DFromFile::Response&);
   void serviceHandlerHandleCameraConnectionLoss();
   bool isConnectedServiceHandler(IsConnected::Request& req, IsConnected::Response& res);
-  void publishFrame(Zivid::Frame&& frame);
+  void publishFrame(const Zivid::Frame& frame);
+  Zivid::Frame invokeCaptureAndPublishFrame();
   bool shouldPublishPointsXYZ() const;
   bool shouldPublishPointsXYZRGBA() const;
   bool shouldPublishColorImg() const;
@@ -97,6 +99,7 @@ class ZividCamera
   ros::ServiceServer camera_info_serial_number_service_;
   ros::ServiceServer camera_info_model_name_service_;
   ros::ServiceServer capture_service_;
+  ros::ServiceServer capture_and_save_frame_service_;
   ros::ServiceServer capture_2d_service_;
   ros::ServiceServer capture_assistant_suggest_settings_service_;
   ros::ServiceServer load_settings_from_file_service_;

zivid_camera/src/zivid_camera.cpp

@@ -240,6 +240,8 @@ ZividCamera::ZividCamera(ros::NodeHandle& nh, ros::NodeHandle& priv)
       nh_.advertiseService("camera_info/serial_number", &ZividCamera::cameraInfoSerialNumberServiceHandler, this);
   is_connected_service_ = nh_.advertiseService("is_connected", &ZividCamera::isConnectedServiceHandler, this);
   capture_service_ = nh_.advertiseService("capture", &ZividCamera::captureServiceHandler, this);
+  capture_and_save_frame_service_ =
+      nh_.advertiseService("capture_and_save_frame", &ZividCamera::captureAndSaveFrameServiceHandler, this);
   capture_2d_service_ = nh_.advertiseService("capture_2d", &ZividCamera::capture2DServiceHandler, this);
   capture_assistant_suggest_settings_service_ = nh_.advertiseService(
       "capture_assistant/suggest_settings", &ZividCamera::captureAssistantSuggestSettingsServiceHandler, this);
@@ -339,18 +341,19 @@ bool ZividCamera::captureServiceHandler(Capture::Request&, Capture::Response&)
 {
   ROS_DEBUG_STREAM(__func__);
 
-  serviceHandlerHandleCameraConnectionLoss();
+  invokeCaptureAndPublishFrame();
 
-  const auto settings = capture_settings_controller_->zividSettings();
+  return true;
+}
 
-  if (settings.acquisitions().isEmpty())
-  {
-    throw std::runtime_error("capture called with 0 enabled acquisitions!");
-  }
+bool ZividCamera::captureAndSaveFrameServiceHandler(CaptureAndSaveFrame::Request& req, CaptureAndSaveFrame::Response&)
+{
+  ROS_DEBUG_STREAM(__func__);
+
+  const auto frame = invokeCaptureAndPublishFrame();
+  ROS_INFO("Saving frame to %s", req.file_path.c_str());
+  frame.save(req.file_path.c_str());
 
-  ROS_INFO("Capturing with %zd acquisition(s)", settings.acquisitions().size());
-  ROS_DEBUG_STREAM(settings);
-  publishFrame(camera_.capture(settings));
   return true;
 }
 
@@ -454,7 +457,7 @@ bool ZividCamera::isConnectedServiceHandler(IsConnected::Request&, IsConnected::
   return true;
 }
 
-void ZividCamera::publishFrame(Zivid::Frame&& frame)
+void ZividCamera::publishFrame(const Zivid::Frame& frame)
 {
   const bool publish_points_xyz = shouldPublishPointsXYZ();
   const bool publish_points_xyzrgba = shouldPublishPointsXYZRGBA();
@@ -694,4 +697,24 @@ sensor_msgs::CameraInfoConstPtr ZividCamera::makeCameraInfo(const std_msgs::Head
   return msg;
 }
 
+Zivid::Frame ZividCamera::invokeCaptureAndPublishFrame()
+{
+  ROS_DEBUG_STREAM(__func__);
+
+  serviceHandlerHandleCameraConnectionLoss();
+
+  const auto settings = capture_settings_controller_->zividSettings();
+
+  if (settings.acquisitions().isEmpty())
+  {
+    throw std::runtime_error("capture called with 0 enabled acquisitions!");
+  }
+
+  ROS_INFO("Capturing with %zd acquisition(s)", settings.acquisitions().size());
+  ROS_DEBUG_STREAM(settings);
+  const auto frame = camera_.capture(settings);
+  publishFrame(frame);
+  return frame;
+}
+
 }  // namespace zivid_camera

zivid_camera/srv/CaptureAndSaveFrame.srv

@@ -0,0 +1,2 @@
+string file_path
+---

zivid_samples/CMakeLists.txt

@@ -43,6 +43,7 @@ function(register_cpp_sample)
 endfunction()
 
 register_cpp_sample(NAME sample_capture_cpp SRC src/sample_capture.cpp)
+register_cpp_sample(NAME sample_capture_and_save_frame_cpp SRC src/sample_capture_and_save_frame.cpp)
 register_cpp_sample(NAME sample_capture_2d_cpp SRC src/sample_capture_2d.cpp)
 register_cpp_sample(NAME sample_capture_assistant_cpp SRC src/sample_capture_assistant.cpp)
 register_cpp_sample(NAME sample_capture_with_settings_from_yml_cpp SRC src/sample_capture_with_settings_from_yml.cpp)
@@ -66,6 +67,7 @@ function(register_python_sample)
 endfunction()
 
 register_python_sample(SRC scripts/sample_capture.py)
+register_python_sample(SRC scripts/sample_capture_and_save_frame.py)
 register_python_sample(SRC scripts/sample_capture_2d.py)
 register_python_sample(SRC scripts/sample_capture_assistant.py)
 register_python_sample(SRC scripts/sample_capture_with_settings_from_yml.py)

zivid_samples/scripts/sample_capture_and_save_frame.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+
+import rospy
+import rosnode
+import dynamic_reconfigure.client
+from zivid_camera.srv import *
+from std_msgs.msg import String
+
+
+class Sample:
+    def __init__(self):
+        rospy.init_node("sample_capture_and_save_frame_py", anonymous=True)
+
+        rospy.loginfo("Starting sample_capture_and_save_frame.py")
+
+        rospy.wait_for_service("/zivid_camera/capture_and_save_frame", 30.0)
+
+        self.capture_and_save_frame_service = rospy.ServiceProxy(
+            "/zivid_camera/capture_and_save_frame", CaptureAndSaveFrame
+        )
+
+        rospy.loginfo("Enabling the first acquisition")
+        acquisition_0_client = dynamic_reconfigure.client.Client(
+            "/zivid_camera/settings/acquisition_0"
+        )
+        acquisition_0_config = {
+            "enabled": True,
+        }
+        acquisition_0_client.update_configuration(acquisition_0_config)
+
+    def capture(self):
+        rospy.loginfo("Calling capture_and_save_frame service")
+        file_path = "/tmp/capture_py.zdf"
+        self.capture_and_save_frame_service(file_path)
+
+
+if __name__ == "__main__":
+    s = Sample()
+    s.capture()
+    rospy.spin()

zivid_samples/src/sample_capture_and_save_frame.cpp

@@ -0,0 +1,62 @@
+#include <zivid_camera/SettingsAcquisitionConfig.h>
+#include <zivid_camera/CaptureAndSaveFrame.h>
+#include <dynamic_reconfigure/Reconfigure.h>
+#include <dynamic_reconfigure/client.h>
+#include <ros/ros.h>
+
+#define CHECK(cmd)                                                                                                     \
+  do                                                                                                                   \
+  {                                                                                                                    \
+    if (!cmd)                                                                                                          \
+    {                                                                                                                  \
+      throw std::runtime_error{ "\"" #cmd "\" failed!" };                                                              \
+    }                                                                                                                  \
+  } while (false)
+
+namespace
+{
+const ros::Duration default_wait_duration{ 30 };
+
+void capture_and_save_frame()
+{
+  ROS_INFO("Calling capture_and_save_frame service");
+  zivid_camera::CaptureAndSaveFrame capture_and_save_frame;
+  std::string file_path = "/tmp/capture_cpp.zdf";
+  capture_and_save_frame.request.file_path = file_path;
+  CHECK(ros::service::call("/zivid_camera/capture_and_save_frame", capture_and_save_frame));
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "sample_capture_and_save_frame_cpp");
+  ros::NodeHandle n;
+
+  ROS_INFO("Starting sample_capture_and_save_frame.cpp");
+
+  CHECK(ros::service::waitForService("/zivid_camera/capture_and_save_frame", default_wait_duration));
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  dynamic_reconfigure::Client<zivid_camera::SettingsAcquisitionConfig> acquisition_0_client("/zivid_camera/settings/"
+                                                                                            "acquisition_0/");
+
+  // To initialize the acquisition_0_config object we need to load the default configuration from the server.
+  // The default values of settings depends on which Zivid camera model is connected.
+  zivid_camera::SettingsAcquisitionConfig acquisition_0_config;
+  CHECK(acquisition_0_client.getDefaultConfiguration(acquisition_0_config, default_wait_duration));
+
+  ROS_INFO("Enabling the first acquisition");
+  acquisition_0_config.enabled = true;
+  CHECK(acquisition_0_client.setConfiguration(acquisition_0_config));
+
+  ROS_INFO("Calling capture_and_save_frame");
+
+  capture_and_save_frame();
+
+  ros::waitForShutdown();
+
+  return 0;
+}