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.

Enable diagnostics mode in the capture so that
Customer Success can better analyze the data.

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

We also include a test.

Closes #65

README.md

@@ -271,6 +271,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
+[zivid_camera/CaptureAndSave.srv](./zivid_camera/srv/CaptureAndSave.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 file extension.
+See [knowledge base](https://support.zivid.com/en/latest/reference-articles/point-cloud-structure-and-output-formats.html#zivid-output-formats)
+for a list of available output formats.
+
+See [Sample Capture and Save](#sample-capture-and-save) for code example.
+
 ### camera_info/model_name
 [zivid_camera/CameraInfoModelName.srv](./zivid_camera/srv/CameraInfoModelName.srv)
 
@@ -611,6 +621,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
+
+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](#capture_and_save) 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.cpp), [Python](./zivid_samples/scripts/sample_capture_and_save.py)
+
+Using roslaunch (also launches `roscore`, `zivid_camera`, `rviz` and `rqt_reconfigure`):
+```bash
+roslaunch zivid_samples sample.launch type:=sample_capture_and_save_cpp
+roslaunch zivid_samples sample.launch type:=sample_capture_and_save.py
+```
+Using rosrun (when `roscore` and `zivid_camera` are running):
+```bash
+rosrun zivid_samples sample_capture_and_save_cpp
+rosrun zivid_samples sample_capture_and_save.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
+  CaptureAndSave.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/CaptureAndSave.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 captureAndSaveServiceHandler(CaptureAndSave::Request& req, CaptureAndSave::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_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_service_ =
+      nh_.advertiseService("capture_and_save", &ZividCamera::captureAndSaveServiceHandler, 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::captureAndSaveServiceHandler(CaptureAndSave::Request& req, CaptureAndSave::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);
 
-  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/CaptureAndSave.srv

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

zivid_camera/test/test_zivid_camera.cpp

@@ -7,6 +7,7 @@
 #include <zivid_camera/CameraInfoSerialNumber.h>
 #include <zivid_camera/CameraInfoModelName.h>
 #include <zivid_camera/Capture.h>
+#include <zivid_camera/CaptureAndSave.h>
 #include <zivid_camera/Capture2D.h>
 #include <zivid_camera/CaptureAssistantSuggestSettings.h>
 #include <zivid_camera/LoadSettingsFromFile.h>
@@ -90,6 +91,7 @@ class ZividNodeTest : public ZividNodeTestBase
   const ros::Duration medium_wait_duration{ 1.0 };
   const ros::Duration dr_get_max_wait_duration{ 5 };
   static constexpr auto capture_service_name = "/zivid_camera/capture";
+  static constexpr auto capture_and_save_service_name = "/zivid_camera/capture_and_save";
   static constexpr auto capture_2d_service_name = "/zivid_camera/capture_2d";
   static constexpr auto capture_assistant_suggest_settings_service_name = "/zivid_camera/capture_assistant/"
                                                                           "suggest_settings";
@@ -185,6 +187,46 @@ class ZividNodeTest : public ZividNodeTestBase
     return SubscriptionWrapper<Type>::make(nh_, name);
   }
 
+  class AllCaptureTopicsSubscriber
+  {
+  public:
+    AllCaptureTopicsSubscriber(ZividNodeTest& nodeTest)
+      : color_camera_info_sub_(nodeTest.subscribe<sensor_msgs::CameraInfo>(color_camera_info_topic_name))
+      , color_image_color_sub_(nodeTest.subscribe<sensor_msgs::Image>(color_image_color_topic_name))
+      , depth_camera_info_sub_(nodeTest.subscribe<sensor_msgs::CameraInfo>(depth_camera_info_topic_name))
+      , depth_image_sub_(nodeTest.subscribe<sensor_msgs::Image>(depth_image_topic_name))
+      , snr_camera_info_sub_(nodeTest.subscribe<sensor_msgs::CameraInfo>(snr_camera_info_topic_name))
+      , snr_image_sub_(nodeTest.subscribe<sensor_msgs::Image>(snr_image_topic_name))
+      , points_xyz_sub_(nodeTest.subscribe<sensor_msgs::PointCloud2>(points_xyz_topic_name))
+      , points_xyzrgba_sub_(nodeTest.subscribe<sensor_msgs::PointCloud2>(points_xyzrgba_topic_name))
+      , normals_xyz_sub_(nodeTest.subscribe<sensor_msgs::PointCloud2>(normals_xyz_topic_name))
+    {
+    }
+
+    SubscriptionWrapper<sensor_msgs::CameraInfo> color_camera_info_sub_;
+    SubscriptionWrapper<sensor_msgs::Image> color_image_color_sub_;
+    SubscriptionWrapper<sensor_msgs::CameraInfo> depth_camera_info_sub_;
+    SubscriptionWrapper<sensor_msgs::Image> depth_image_sub_;
+    SubscriptionWrapper<sensor_msgs::CameraInfo> snr_camera_info_sub_;
+    SubscriptionWrapper<sensor_msgs::Image> snr_image_sub_;
+    SubscriptionWrapper<sensor_msgs::PointCloud2> points_xyz_sub_;
+    SubscriptionWrapper<sensor_msgs::PointCloud2> points_xyzrgba_sub_;
+    SubscriptionWrapper<sensor_msgs::PointCloud2> normals_xyz_sub_;
+
+    void assert_num_topics_received(std::size_t numTopics)
+    {
+      ASSERT_EQ(color_camera_info_sub_.numMessages(), numTopics);
+      ASSERT_EQ(color_image_color_sub_.numMessages(), numTopics);
+      ASSERT_EQ(depth_camera_info_sub_.numMessages(), numTopics);
+      ASSERT_EQ(depth_image_sub_.numMessages(), numTopics);
+      ASSERT_EQ(snr_camera_info_sub_.numMessages(), numTopics);
+      ASSERT_EQ(snr_image_sub_.numMessages(), numTopics);
+      ASSERT_EQ(points_xyz_sub_.numMessages(), numTopics);
+      ASSERT_EQ(points_xyzrgba_sub_.numMessages(), numTopics);
+      ASSERT_EQ(normals_xyz_sub_.numMessages(), numTopics);
+    }
+  };
+
   template <class A, class B>
   void assertArrayFloatEq(const A& actual, const B& expected) const
   {
@@ -305,53 +347,33 @@ TEST_F(ZividNodeTest, testServiceIsConnected)
 
 TEST_F(ZividNodeTest, testCapturePublishesTopics)
 {
-  auto color_camera_info_sub = subscribe<sensor_msgs::CameraInfo>(color_camera_info_topic_name);
-  auto color_image_color_sub = subscribe<sensor_msgs::Image>(color_image_color_topic_name);
-  auto depth_camera_info_sub = subscribe<sensor_msgs::CameraInfo>(depth_camera_info_topic_name);
-  auto depth_image_sub = subscribe<sensor_msgs::Image>(depth_image_topic_name);
-  auto snr_camera_info_sub = subscribe<sensor_msgs::CameraInfo>(snr_camera_info_topic_name);
-  auto snr_image_sub = subscribe<sensor_msgs::Image>(snr_image_topic_name);
-  auto points_xyz_sub = subscribe<sensor_msgs::PointCloud2>(points_xyz_topic_name);
-  auto points_xyzrgba_sub = subscribe<sensor_msgs::PointCloud2>(points_xyzrgba_topic_name);
-  auto normals_xyz_sub = subscribe<sensor_msgs::PointCloud2>(normals_xyz_topic_name);
-
-  auto assert_num_topics_received = [&](std::size_t numTopics) {
-    ASSERT_EQ(color_camera_info_sub.numMessages(), numTopics);
-    ASSERT_EQ(color_image_color_sub.numMessages(), numTopics);
-    ASSERT_EQ(depth_camera_info_sub.numMessages(), numTopics);
-    ASSERT_EQ(depth_image_sub.numMessages(), numTopics);
-    ASSERT_EQ(snr_camera_info_sub.numMessages(), numTopics);
-    ASSERT_EQ(snr_image_sub.numMessages(), numTopics);
-    ASSERT_EQ(points_xyz_sub.numMessages(), numTopics);
-    ASSERT_EQ(points_xyzrgba_sub.numMessages(), numTopics);
-    ASSERT_EQ(normals_xyz_sub.numMessages(), numTopics);
-  };
+  AllCaptureTopicsSubscriber all_capture_topics_subscriber(*this);
 
   medium_wait_duration.sleep();
-  assert_num_topics_received(0);
+  all_capture_topics_subscriber.assert_num_topics_received(0);
 
   zivid_camera::Capture capture;
   // Capture fails when no acquisitions are enabled
   ASSERT_FALSE(ros::service::call(capture_service_name, capture));
   short_wait_duration.sleep();
-  assert_num_topics_received(0);
+  all_capture_topics_subscriber.assert_num_topics_received(0);
 
   enableFirst3DAcquisition();
 
   ASSERT_TRUE(ros::service::call(capture_service_name, capture));
   short_wait_duration.sleep();
-  assert_num_topics_received(1);
+  all_capture_topics_subscriber.assert_num_topics_received(1);
 
   ASSERT_TRUE(ros::service::call(capture_service_name, capture));
   short_wait_duration.sleep();
-  assert_num_topics_received(2);
+  all_capture_topics_subscriber.assert_num_topics_received(2);
 
   ASSERT_TRUE(ros::service::call(capture_service_name, capture));
   short_wait_duration.sleep();
-  assert_num_topics_received(3);
+  all_capture_topics_subscriber.assert_num_topics_received(3);
 
   short_wait_duration.sleep();
-  assert_num_topics_received(3);
+  all_capture_topics_subscriber.assert_num_topics_received(3);
 }
 
 class CaptureOutputTest : public TestWithFileCamera
@@ -730,6 +752,63 @@ TEST_F(CaptureOutputTest, testCapture2D)
   verify_image_and_camera_info(*color_image_color_sub.lastMessage(), *color_camera_info_sub.lastMessage());
 }
 
+class CaptureAndSaveTest : public TestWithFileCamera
+{
+protected:
+  void capture_and_save_to_path(const std::string& file_path, bool expect_success)
+  {
+    AllCaptureTopicsSubscriber all_capture_topics_subscriber(*this);
+    medium_wait_duration.sleep();
+
+    enableFirst3DAcquisition();
+    zivid_camera::CaptureAndSave capture_and_save;
+    capture_and_save.request.file_path = file_path;
+    all_capture_topics_subscriber.assert_num_topics_received(0);
+    if (expect_success)
+    {
+      ASSERT_TRUE(ros::service::call(capture_and_save_service_name, capture_and_save));
+      ASSERT_TRUE(boost::filesystem::exists(file_path));
+    }
+    else
+    {
+      ASSERT_FALSE(ros::service::call(capture_and_save_service_name, capture_and_save));
+      ASSERT_FALSE(boost::filesystem::exists(file_path));
+    }
+    medium_wait_duration.sleep();
+    all_capture_topics_subscriber.assert_num_topics_received(1);
+  }
+};
+
+TEST_F(CaptureAndSaveTest, testCaptureAndSaveNoPath)
+{
+  capture_and_save_to_path("", false);
+}
+
+TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidPath)
+{
+  capture_and_save_to_path("invalid_path", false);
+}
+
+TEST_F(CaptureAndSaveTest, testCaptureAndSaveInvalidExtension)
+{
+  capture_and_save_to_path("/tmp/invalid_extension.wrong", false);
+}
+
+TEST_F(CaptureAndSaveTest, testCaptureAndSaveZDF)
+{
+  capture_and_save_to_path("/tmp/valid.zdf", true);
+}
+
+TEST_F(CaptureAndSaveTest, testCaptureAndSavePLY)
+{
+  capture_and_save_to_path("/tmp/valid.ply", true);
+}
+
+TEST_F(CaptureAndSaveTest, testCaptureAndSavePCD)
+{
+  capture_and_save_to_path("/tmp/valid.pcd", true);
+}
+
 class DynamicReconfigureMinMaxDefaultTest : public TestWithFileCamera
 {
 protected:

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_cpp SRC src/sample_capture_and_save.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.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)