Merge pull request #32 from zivid/support_sdk2

Support sdk2

CHANGELOG.md

@@ -2,6 +2,35 @@
 
 This project adheres to [Semantic Versioning](https://semver.org).
 
+## 2.0.0
+
+* Support for version 2.0.0 of the Zivid SDK. For a full list of changes in the SDK, see the
+  [SDK 2.0.0 Changelog](https://www.zivid.com/software/releases/2.0.0+7c4918cf-14/Changelog.md).
+  Note that Zivid SDK 1.x is not supported by this driver version.
+* The "points" topic has been renamed to "points/xyzrgba". The "contrast" value is no longer
+  available in this message. Use the SNR image at topic "snr/image" instead.
+* Added new topic "points/xyz". This is similar to "points/xyzrgba" except the color values
+  are not included in the message. The message is compatible with pcl::PointXYZ. This topic is
+  recommended for users who do not use the RGBA values in the point cloud.
+* Added new "snr/image" topic. This topic contains the SNR values for all the pixels. SNR
+  replaces the old "Contrast" values from SDK 1.x.
+* Changes to how capture settings are configured:
+  * The available settings, and the names and paths of the settings, has been updated to match
+    the SDK 2.0.0 API. Note that some settings have been removed in 2.0.0. In addition the default
+    values for some settings have changed. See the [README](./README.md) for a table of the
+    supported settings in 2.0.0. Refer to the
+    [SDK 2.0.0 Changelog](https://www.zivid.com/software/releases/2.0.0+7c4918cf-14/Changelog.md)
+    for all the changes to settings.
+  * The dynamic_reconfigure node `capture/general/` has been renamed to `settings/`.
+  * The dynamic_reconfigure nodes `capture/frame_<n>/` has been renamed to `settings/acquisition_<n>/`.
+  * The dynamic_reconfigure node `capture_2d/frame_0/` has been renamed to `settings_2d/acquisition_0/`.
+  * Added new dynamic_reconfigure node `settings_2d/` which contains non-acquisition related 2D settings.
+    As of SDK 2.0.0 this contains color balance settings.
+* The depth image topic has been renamed from "depth/image_raw" to "depth/image".
+* The encoding of "color/image" for 3D captures has been changed from "rgb8" to "rgba8".
+* The launch parameter "num_capture_frames" has been renamed to "max_capture_acquisitions" to be
+  consistent with the naming in the Zivid SDK.
+
 ## 1.1.0
 
 * Updated the Intel OpenCL CPU runtime driver used in CI tests. This makes the CI tests

azure-pipelines.yml

@@ -6,7 +6,7 @@ schedules:
     - master
   always: true
 variables:
-  newest_zivid_version: '1.8.1+6967bc1b-1'
+  newest_zivid_version: '2.0.0+7c4918cf-14'
 stages:
   - stage: Code_Analysis
     jobs:
@@ -29,10 +29,13 @@ stages:
         zivid_versions: ["${{ variables['newest_zivid_version'] }}"]
         ros_distros: ['ros:kinetic-ros-base-xenial']
         compilers: ['g++-7']
-  - stage: Build_And_Test_Old_Zivid_Melodic_And_Kinetic
-    jobs:
-    - template: continuous-integration/azure-pipelines/templates/build_and_test.yml
-      parameters:
-        zivid_versions: ['1.8.0+89ae8b3e-39']
-        ros_distros: ['ros:melodic-ros-base-bionic', 'ros:kinetic-ros-base-xenial']
-        compilers: ['g++-7']
+# Currently only newest_zivid_version is supported. Azure Pipelines requires at least one job under
+# jobs, so parameters.zivid_versions cannot be empty. Re-enable this when we support >0 old versions.
+#
+#  - stage: Build_And_Test_Old_Zivid_Melodic_And_Kinetic
+#    jobs:
+#    - template: continuous-integration/azure-pipelines/templates/build_and_test.yml
+#      parameters:
+#        zivid_versions: ['']
+#        ros_distros: ['ros:melodic-ros-base-bionic', 'ros:kinetic-ros-base-xenial']
+#        compilers: ['g++-7']

continuous-integration/ZividAPIConfigCPU.yml

@@ -1,20 +1,4 @@
 __version__: 4
 Configuration:
   ComputeDevice:
-    SelectionFilter: ""
     Type: CPU
-    Vendor: Any
-  DriverConfig:
-    ToshibaTeli:
-      VersionCheck: yes
-  Logging:
-    Asynchronous: yes
-    CrashHandler: yes
-    Directory: ""
-    Enabled: yes
-    FlushOnExit: yes
-    HistorySize: 30
-    MaxFileSizeMB: 50
-    MaxTotalSizeMB: 1000
-    RotationCount: 0
-    WriteInterval: 200

continuous-integration/setup/setup_build_and_test.sh

@@ -88,11 +88,7 @@ fi
 echo "Install Zivid and Telicam debian packages"
 
 ZIVID_RELEASE_DIR="https://www.zivid.com/hubfs/softwarefiles/releases/$CI_TEST_ZIVID_VERSION"
-ZIVID_TELICAM_SDK_DEB="zivid-telicam-driver_3.0.1.1-1_amd64.deb"
-
-if [[ "$CI_TEST_ZIVID_VERSION" =~ ^1\.8\.0.* ]]; then
-    ZIVID_TELICAM_SDK_DEB="zivid-telicam-driver_2.0.0.1-1_amd64.deb"
-fi
+ZIVID_TELICAM_SDK_DEB="zivid-telicam-driver_3.0.1.1-3_amd64.deb"
 
 if [[ "$UBUNTU_VERSION" == "16.04" ]]; then
 

continuous-integration/test.sh

@@ -10,10 +10,10 @@ echo "Installing Zivid API config file"
 install -D "$SCRIPT_DIR"/ZividAPIConfigCPU.yml "$HOME"/.config/Zivid/API/Config.yml || exit $?
 
 echo "Download and install zivid sample data (file camera)"
-wget -q https://www.zivid.com/software/ZividSampleData.zip || exit $?
+wget -q https://www.zivid.com/software/FileCameraZividOne.zip || exit $?
 mkdir -p /usr/share/Zivid/data/ || exit $?
-unzip ./ZividSampleData.zip -d /usr/share/Zivid/data/ || exit $?
-rm ./ZividSampleData.zip || exit $?
+unzip ./FileCameraZividOne.zip -d /usr/share/Zivid/data/ || exit $?
+rm ./FileCameraZividOne.zip || exit $?
 
 echo "Running tests"
 catkin run_tests -DCOMPILER_WARNINGS=ON -DCMAKE_CXX_COMPILER=/usr/bin/$CI_TEST_COMPILER  || exit $?

zivid_camera/CMakeLists.txt

@@ -30,7 +30,9 @@ find_package(catkin REQUIRED COMPONENTS
   nodelet
 )
 
-find_package(Zivid 1.8.0 COMPONENTS Core REQUIRED)
+find_package(Boost REQUIRED COMPONENTS filesystem)
+
+find_package(Zivid 2.0.0 COMPONENTS Core REQUIRED)
 message(STATUS "Found Zivid version ${Zivid_VERSION}")
 
 find_package(OpenMP REQUIRED)
@@ -45,33 +47,36 @@ target_include_directories(
   SYSTEM PRIVATE
   ${catkin_INCLUDE_DIRS}
 )
-target_link_libraries(${SETTINGS_GENERATOR_TARGET_NAME} PRIVATE Zivid::Core)
-
+target_link_libraries(${SETTINGS_GENERATOR_TARGET_NAME} PRIVATE Zivid::Core Boost::filesystem)
 
 set(GENERATOR_TARGET_NAME ${PROJECT_NAME}_generator)
 add_custom_target(
   ${GENERATOR_TARGET_NAME}
   COMMAND mkdir -p ${CMAKE_CURRENT_BINARY_DIR}/state_srv_files
   COMMAND mkdir -p ${CMAKE_CURRENT_BINARY_DIR}/generated_headers
   COMMAND ${SETTINGS_GENERATOR_TARGET_NAME}
-  COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/CaptureFrame.cfg
-  COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/CaptureGeneral.cfg
-  COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/Capture2DFrame.cfg
-  BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/CaptureFrame.cfg
-             ${CMAKE_CURRENT_BINARY_DIR}/CaptureGeneral.cfg
-             ${CMAKE_CURRENT_BINARY_DIR}/Capture2DFrame.cfg
-             ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/CaptureFrameConfigUtils.h
-             ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/CaptureGeneralConfigUtils.h
-             ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/Capture2DFrameConfigUtils.h
+  COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/SettingsAcquisition.cfg
+  COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/Settings.cfg
+  COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/Settings2DAcquisition.cfg
+  COMMAND chmod a+x ${CMAKE_CURRENT_BINARY_DIR}/Settings2D.cfg
+  BYPRODUCTS ${CMAKE_CURRENT_BINARY_DIR}/SettingsAcquisition.cfg
+             ${CMAKE_CURRENT_BINARY_DIR}/Settings.cfg
+             ${CMAKE_CURRENT_BINARY_DIR}/Settings2DAcquisition.cfg
+             ${CMAKE_CURRENT_BINARY_DIR}/Settings2D.cfg
+             ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/SettingsAcquisitionConfigUtils.h
+             ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/SettingsConfigUtils.h
+             ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/Settings2DAcquisitionConfigUtils.h
+             ${CMAKE_CURRENT_BINARY_DIR}/generated_headers/Settings2DConfigUtils.h
   COMMENT "Running generator"
 )
 
 set(LIBRARY_NAME ${PROJECT_NAME})
 
 generate_dynamic_reconfigure_options(
-  ${CMAKE_CURRENT_BINARY_DIR}/CaptureGeneral.cfg
-  ${CMAKE_CURRENT_BINARY_DIR}/CaptureFrame.cfg
-  ${CMAKE_CURRENT_BINARY_DIR}/Capture2DFrame.cfg
+  ${CMAKE_CURRENT_BINARY_DIR}/Settings.cfg
+  ${CMAKE_CURRENT_BINARY_DIR}/SettingsAcquisition.cfg
+  ${CMAKE_CURRENT_BINARY_DIR}/Settings2D.cfg
+  ${CMAKE_CURRENT_BINARY_DIR}/Settings2DAcquisition.cfg
 )
 add_dependencies(${PROJECT_NAME}_gencfg ${GENERATOR_TARGET_NAME})
 add_service_files(
@@ -102,7 +107,11 @@ catkin_package(
 set_property(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY INCLUDE_DIRECTORIES "")
 
 # Library
-add_library(${LIBRARY_NAME} src/zivid_camera.cpp)
+add_library(
+  ${LIBRARY_NAME}
+  src/zivid_camera.cpp
+  src/capture_settings_controller.cpp
+)
 turn_on_compiler_warnings_if_enabled(${LIBRARY_NAME})
 target_include_directories(
   ${LIBRARY_NAME}

zivid_camera/include/auto_generated_include_wrapper.h

@@ -7,9 +7,10 @@
 #pragma GCC system_header
 #endif
 
-#include <zivid_camera/CaptureFrameConfig.h>
-#include <zivid_camera/CaptureGeneralConfig.h>
-#include <zivid_camera/Capture2DFrameConfig.h>
+#include <zivid_camera/SettingsAcquisitionConfig.h>
+#include <zivid_camera/SettingsConfig.h>
+#include <zivid_camera/Settings2DAcquisitionConfig.h>
+#include <zivid_camera/Settings2DConfig.h>
 #include <zivid_camera/Capture.h>
 #include <zivid_camera/Capture2D.h>
 #include <zivid_camera/CaptureAssistantSuggestSettings.h>

zivid_camera/include/capture_settings_controller.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include "auto_generated_include_wrapper.h"
+
+#include <Zivid/Settings.h>
+#include <Zivid/Settings2D.h>
+
+#include <ros/node_handle.h>
+
+namespace Zivid
+{
+class Camera;
+}
+
+namespace zivid_camera
+{
+template <typename ConfigType, typename ZividSettings>
+class ConfigDRServer;
+
+/// <summary>Controller that manages dynamic_reconfigure nodes for Settings and Settings2D</summary>
+/// <remarks>
+/// This is a templated class that handles Zivid::Settings and Zivid::Settings2D configuration. Note that
+/// within the Zivid SDK Settings and Settings2D are similar in structure, but there are some differences
+/// in what kind of settings are provided.
+///
+/// This controller sets up the dynamic_reconfigure nodes "settings/", "settings/acquisition_0" and so
+/// on, handles callbacks and updates the internal config state. It also provides methods to convert
+/// from/to Zivid::Settings/Settings2D objects.
+/// </remarks>
+template <typename ZividSettingsType, typename SettingsConfigType, typename SettingsAcquisitionConfigType>
+class CaptureSettingsController
+{
+  static_assert(std::is_same_v<ZividSettingsType, Zivid::Settings> ||
+                std::is_same_v<ZividSettingsType, Zivid::Settings2D>);
+  static_assert(std::is_same_v<SettingsConfigType, SettingsConfig> ||
+                std::is_same_v<SettingsConfigType, Settings2DConfig>);
+  static_assert(std::is_same_v<SettingsAcquisitionConfigType, SettingsAcquisitionConfig> ||
+                std::is_same_v<SettingsAcquisitionConfigType, Settings2DAcquisitionConfig>);
+
+public:
+  CaptureSettingsController(ros::NodeHandle& nh, Zivid::Camera& camera, const std::string& config_node_name,
+                            std::size_t num_acquisition_servers);
+  ~CaptureSettingsController();
+  ZividSettingsType zividSettings() const;
+  void setZividSettings(const ZividSettingsType& settings);
+  std::size_t numAcquisitionConfigServers() const;
+
+private:
+  using SettingsConfigTypeDRServer = ConfigDRServer<SettingsConfigType, ZividSettingsType>;
+  using SettingsAcquisitionConfigTypeDRServer =
+      ConfigDRServer<SettingsAcquisitionConfigType, typename ZividSettingsType::Acquisition>;
+  std::string config_node_name_;
+  std::unique_ptr<SettingsConfigTypeDRServer> general_config_dr_server_;
+  std::vector<std::unique_ptr<SettingsAcquisitionConfigTypeDRServer>> acquisition_config_dr_servers_;
+};
+
+extern template class CaptureSettingsController<Zivid::Settings, zivid_camera::SettingsConfig,
+                                                zivid_camera::SettingsAcquisitionConfig>;
+extern template class CaptureSettingsController<Zivid::Settings2D, zivid_camera::Settings2DConfig,
+                                                zivid_camera::Settings2DAcquisitionConfig>;
+
+}  // namespace zivid_camera

zivid_camera/include/config_utils_common.h

@@ -6,7 +6,9 @@
 
 template <typename ConfigType, typename ZividSettings>
 ConfigType zividSettingsToConfig(const ZividSettings& s) = delete;
+
 template <typename ConfigType, typename ZividSettings>
-ConfigType zividSettingsToMinConfig(const ZividSettings& s) = delete;
+ConfigType zividSettingsMinConfig(const Zivid::Camera& camera) = delete;
+
 template <typename ConfigType, typename ZividSettings>
-ConfigType zividSettingsToMaxConfig(const ZividSettings& s) = delete;
+ConfigType zividSettingsMaxConfig(const Zivid::Camera& camera) = delete;

zivid_camera/include/zivid_camera.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "auto_generated_include_wrapper.h"
+#include "capture_settings_controller.h"
 
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/Image.h>
@@ -42,72 +43,59 @@ class ZividCamera
   bool cameraInfoSerialNumberServiceHandler(CameraInfoSerialNumber::Request& req,
                                             CameraInfoSerialNumber::Response& res);
   bool captureServiceHandler(Capture::Request& req, Capture::Response& res);
-  bool capture2DServiceHandler(Capture::Request& req, Capture::Response& res);
+  bool capture2DServiceHandler(Capture2D::Request& req, Capture2D::Response& res);
   bool captureAssistantSuggestSettingsServiceHandler(CaptureAssistantSuggestSettings::Request& req,
                                                      CaptureAssistantSuggestSettings::Response& res);
   void serviceHandlerHandleCameraConnectionLoss();
   bool isConnectedServiceHandler(IsConnected::Request& req, IsConnected::Response& res);
   void publishFrame(Zivid::Frame&& frame);
-  bool shouldPublishPoints() const;
+  bool shouldPublishPointsXYZ() const;
+  bool shouldPublishPointsXYZRGBA() const;
   bool shouldPublishColorImg() const;
   bool shouldPublishDepthImg() const;
+  bool shouldPublishSnrImg() const;
   std_msgs::Header makeHeader();
-  sensor_msgs::PointCloud2ConstPtr makePointCloud2(const std_msgs::Header& header,
-                                                   const Zivid::PointCloud& point_cloud);
-  sensor_msgs::ImageConstPtr makeColorImage(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud);
-  sensor_msgs::ImageConstPtr makeColorImage(const std_msgs::Header& header, const Zivid::Image<Zivid::RGBA8>& image);
-  sensor_msgs::ImageConstPtr makeDepthImage(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud);
+  void publishPointCloudXYZ(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud);
+  void publishPointCloudXYZRGBA(const std_msgs::Header& header, const Zivid::PointCloud& point_cloud);
+  void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info,
+                         const Zivid::PointCloud& point_cloud);
+  void publishColorImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info,
+                         const Zivid::Image<Zivid::ColorRGBA>& image);
+  void publishDepthImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info,
+                         const Zivid::PointCloud& point_cloud);
+  void publishSnrImage(const std_msgs::Header& header, const sensor_msgs::CameraInfoConstPtr& camera_info,
+                       const Zivid::PointCloud& point_cloud);
   sensor_msgs::CameraInfoConstPtr makeCameraInfo(const std_msgs::Header& header, std::size_t width, std::size_t height,
                                                  const Zivid::CameraIntrinsics& intrinsics);
 
-  template <typename ConfigType_>
-  class ConfigDRServer
-  {
-  public:
-    using ConfigType = ConfigType_;
-    template <typename ZividSettings>
-    ConfigDRServer(const std::string& name, ros::NodeHandle& nh, const ZividSettings& defaultSettings);
-    void setConfig(const ConfigType& cfg);
-    const ConfigType& config() const
-    {
-      return config_;
-    }
-    const std::string& name() const
-    {
-      return name_;
-    }
-
-  private:
-    std::string name_;
-    boost::recursive_mutex dr_server_mutex_;
-    dynamic_reconfigure::Server<ConfigType> dr_server_;
-    ConfigType config_;
-  };
-
-  using CaptureGeneralConfigDRServer = ConfigDRServer<CaptureGeneralConfig>;
-  using CaptureFrameConfigDRServer = ConfigDRServer<CaptureFrameConfig>;
-  using Capture2DFrameConfigDRServer = ConfigDRServer<Capture2DFrameConfig>;
+  using Capture3DSettingsController =
+      CaptureSettingsController<Zivid::Settings, SettingsConfig, SettingsAcquisitionConfig>;
+  using Capture2DSettingsController =
+      CaptureSettingsController<Zivid::Settings2D, Settings2DConfig, Settings2DAcquisitionConfig>;
 
   ros::NodeHandle nh_;
   ros::NodeHandle priv_;
   ros::Timer camera_connection_keepalive_timer_;
   CameraStatus camera_status_;
-  std::unique_ptr<CaptureGeneralConfigDRServer> capture_general_config_dr_server_;
-  bool use_latched_publisher_for_points_;
+  bool use_latched_publisher_for_points_xyz_;
+  bool use_latched_publisher_for_points_xyzrgba_;
   bool use_latched_publisher_for_color_image_;
   bool use_latched_publisher_for_depth_image_;
-  ros::Publisher points_publisher_;
+  bool use_latched_publisher_for_snr_image_;
+  ros::Publisher points_xyz_publisher_;
+  ros::Publisher points_xyzrgba_publisher_;
   image_transport::ImageTransport image_transport_;
   image_transport::CameraPublisher color_image_publisher_;
   image_transport::CameraPublisher depth_image_publisher_;
+  image_transport::CameraPublisher snr_image_publisher_;
   ros::ServiceServer camera_info_serial_number_service_;
   ros::ServiceServer camera_info_model_name_service_;
   ros::ServiceServer capture_service_;
   ros::ServiceServer capture_2d_service_;
   ros::ServiceServer capture_assistant_suggest_settings_service_;
   ros::ServiceServer is_connected_service_;
-  std::vector<std::unique_ptr<CaptureFrameConfigDRServer>> capture_frame_config_dr_servers_;
-  std::vector<std::unique_ptr<Capture2DFrameConfigDRServer>> capture_2d_frame_config_dr_servers_;
+  std::unique_ptr<Capture3DSettingsController> capture_settings_controller_;
+  std::unique_ptr<Capture2DSettingsController> capture_2d_settings_controller_;
   Zivid::Application zivid_;
   Zivid::Camera camera_;
   std::string frame_id_;

zivid_camera/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>zivid_camera</name>
-  <version>1.1.0</version>
+  <version>2.0.0</version>
   <description>Driver for using the Zivid 3D cameras in ROS.</description>
   <maintainer email="[email protected]">Zivid</maintainer>
   <license>BSD3</license>