Merge pull request #38 from zivid/support_for_sdk_2.2

Support for sdk 2.2

CHANGELOG.md

@@ -2,6 +2,10 @@
 
 This project adheres to [Semantic Versioning](https://semver.org).
 
+## 2.2.0
+
+* Add support for version 2.2.0 of the Zivid SDK.
+
 ## 2.0.0
 
 * Support for version 2.0.0 of the Zivid SDK. For a full list of changes in the SDK, see the

azure-pipelines.yml

@@ -6,7 +6,7 @@ schedules:
     - master
   always: true
 variables:
-  newest_zivid_version: '2.1.0+d2007e12-1'
+  newest_zivid_version: '2.2.0+f0867d62-1'
 stages:
   - stage: Code_Analysis
     jobs:
@@ -33,6 +33,6 @@ stages:
     jobs:
     - template: continuous-integration/azure-pipelines/templates/build_and_test.yml
       parameters:
-        zivid_versions: ['2.0.0+7c4918cf-14']
+        zivid_versions: ['2.0.0+7c4918cf-14', '2.1.0+d2007e12-1']
         ros_distros: ['ros:melodic-ros-base-bionic', 'ros:kinetic-ros-base-xenial']
         compilers: ['g++-7']

zivid_camera/package.xml

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

zivid_camera/src/settings_generator.cpp

@@ -10,14 +10,17 @@
 
 #include <boost/algorithm/string.hpp>
 #include <boost/algorithm/string/replace.hpp>
+#include <boost/algorithm/string/join.hpp>
 #include <boost/filesystem.hpp>
+#include <boost/format.hpp>
 
 #include <algorithm>
 #include <cctype>
 #include <chrono>
 #include <fstream>
 #include <regex>
 #include <sstream>
+#include <stdexcept>
 #include <string>
 
 namespace
@@ -46,6 +49,11 @@ struct IsInTuple<T, std::tuple<Ts...>> : IsInList<T, Ts...>
 {
 };
 
+template <typename SettingsNode>
+struct IsEnumSetting : std::is_enum<typename SettingsNode::ValueType>
+{
+};
+
 void writeToFile(const std::string& file_name, const std::string& text)
 {
   if (boost::filesystem::exists(file_name))
@@ -78,6 +86,16 @@ void writeToFile(const std::string& file_name, const std::string& text)
   }
 }
 
+std::string toUpperCaseFirst(std::string value)
+{
+  if (value.empty())
+  {
+    throw std::invalid_argument("value is empty");
+  }
+  value[0] = std::toupper(value[0]);
+  return value;
+}
+
 template <typename SettingsRootGroup, typename SettingsNode>
 std::string convertSettingsPathToConfigPath()
 {
@@ -136,6 +154,30 @@ std::string fullyQualifiedZividTypeName()
   return ss.str();
 }
 
+template <typename SettingsNode>
+std::string settingEnumValueToRosEnumName(typename SettingsNode::ValueType value)
+{
+  // During build dynamic_reconfigure will auto-generate variables for C++ and Python
+  // for all enum values, scoped directly under the top level .cfg name. So, for example,
+  // an enum value named X listed in Settings.cfg would auto-generate an const int variable
+  // zivid_camera::Settings_X. Since we may have several different enum settings with the
+  // same enum value X, we need to scope the enum name with the full path to the setting.
+  // This ensures there are no collisions now or in the future.
+  const auto value_str = toUpperCaseFirst(SettingsNode{ value }.toString());
+  const auto path = boost::replace_all_copy<std::string>(SettingsNode::path, "/", "");
+  return path + value_str;
+}
+
+template <typename SettingsNode>
+std::string generateEnumConstant(typename SettingsNode::ValueType value)
+{
+  static_assert(IsEnumSetting<SettingsNode>::value);
+  const auto name = settingEnumValueToRosEnumName<SettingsNode>(value);
+  const auto int_value = static_cast<std::size_t>(value);
+  const auto description = toUpperCaseFirst(SettingsNode{ value }.toString());
+  return (boost::format(R"(gen.const("%1%", int_t, %2%, "%3%"))") % name % int_value % description).str();
+}
+
 template <typename SettingsRootGroup>
 class DynamicReconfigureCfgGenerator
 {
@@ -159,6 +201,10 @@ class DynamicReconfigureCfgGenerator
     {
       return SettingsNode::validRange().min();
     }
+    if constexpr (IsEnumSetting<SettingsNode>::value)
+    {
+      return *SettingsNode::validValues().begin();
+    }
     return ValueType{ 0 };
   }
 
@@ -179,6 +225,10 @@ class DynamicReconfigureCfgGenerator
     {
       return static_cast<int>(value.count());
     }
+    else if constexpr (std::is_enum_v<ValueType>)
+    {
+      return static_cast<int>(value);
+    }
     else
     {
       static_assert(DependentFalse<ValueType>::value, "Could not convert ValueType to ROS type.");
@@ -229,6 +279,11 @@ class DynamicReconfigureCfgGenerator
     return rosTypeToString(convertValueToRosValue(v));
   }
 
+  std::string rosGeneratedEnumVariableName(const std::string& setting_name)
+  {
+    return setting_name + "_enum";
+  }
+
   template <typename SettingsNode>
   void apply(const SettingsNode& node)
   {
@@ -240,6 +295,16 @@ class DynamicReconfigureCfgGenerator
     const auto type_name = rosTypeName<decltype(convertValueToRosValue(default_value))>();
     const auto default_value_str = valueTypeToRosTypeString(default_value);
 
+    if constexpr (IsEnumSetting<SettingsNode>::value)
+    {
+      const auto valid_values = node.validValues();
+      std::vector<std::string> enum_constants;
+      std::transform(valid_values.cbegin(), valid_values.cend(), std::back_inserter(enum_constants),
+                     [&](const auto value) { return generateEnumConstant<SettingsNode>(value); });
+      ss_ << rosGeneratedEnumVariableName(setting_name) << " = gen.enum([\n    "
+          << boost::algorithm::join(enum_constants, ",\n    ") << "\n],\n \"" << description << "\")\n";
+    }
+
     ss_ << "gen.add(\"" << setting_name << "\", " << type_name << ", " << level << ", "
         << "\"" << description << "\", " << default_value_str;
 
@@ -248,6 +313,12 @@ class DynamicReconfigureCfgGenerator
       ss_ << ", " << valueTypeToRosTypeString(node.validRange().min()) << ", "
           << valueTypeToRosTypeString(node.validRange().max());
     }
+    else if constexpr (IsEnumSetting<SettingsNode>::value)
+    {
+      const auto min_index = 0;
+      const auto max_index = node.validValues().size() - 1;
+      ss_ << ", " << min_index << ", " << max_index << ", edit_method=" << rosGeneratedEnumVariableName(setting_name);
+    }
     ss_ << ")\n";
   }
 
@@ -292,7 +363,7 @@ class ApplyConfigToZividSettingsGenerator
   }
 
   template <typename SettingsNode>
-  void apply(const SettingsNode&)
+  void apply(const SettingsNode& node)
   {
     using ValueType = typename SettingsNode::ValueType;
 
@@ -308,6 +379,23 @@ class ApplyConfigToZividSettingsGenerator
     {
       ss_ << "std::chrono::microseconds(" + cfg_id + ")";
     }
+    else if constexpr (IsEnumSetting<SettingsNode>::value)
+    {
+      ss_ << "\n"
+          << "  [value = " + cfg_id + "](){\n"
+          << "    switch(value) {\n";
+      const auto valid_values = node.validValues();
+      for (const auto& enum_value : valid_values)
+      {
+        ss_ << "    case zivid_camera::" << config_class_name_ << "_"
+            << settingEnumValueToRosEnumName<SettingsNode>(enum_value) << ":\n"
+            << "      return " + setting_node_class_name + "::" + SettingsNode{ enum_value }.toString() + ";\n";
+      }
+      ss_ << "    };\n"
+          << "    throw std::runtime_error(\"Could not convert int value \" + std::to_string(value) + \""
+          << " to setting of type " + setting_node_class_name + ".\");\n"
+          << "  }()\n";
+    }
     else
     {
       ss_ << cfg_id;
@@ -357,6 +445,10 @@ class ZividSettingsToConfigGenerator
     {
       ss_ << "static_cast<int>(" + value_str + ");\n";
     }
+    else if constexpr (IsEnumSetting<SettingsNode>::value)
+    {
+      ss_ << "static_cast<int>(" + value_str + ");\n";
+    }
     else
     {
       ss_ << value_str + ";\n";

zivid_camera/test/test_zivid_camera.cpp

@@ -42,7 +42,17 @@ ros::Duration toRosDuration(const std::chrono::duration<Rep, Period>& d)
 }
 }  // namespace
 
-class ZividNodeTest : public testing::Test
+class ZividNodeTestBase : public testing::Test
+{
+protected:
+  ZividNodeTestBase()
+  {
+    const auto test_name = testing::UnitTest::GetInstance()->current_test_info()->name();
+    std::cerr << "Starting test " << test_name << "\n";
+  }
+};
+
+class ZividNodeTest : public ZividNodeTestBase
 {
 protected:
   ros::NodeHandle nh_;
@@ -457,6 +467,36 @@ TEST_F(CaptureOutputTest, testCaptureSNRImage)
   }
 }
 
+#if ZIVID_CORE_VERSION_MAJOR > 2 || (ZIVID_CORE_VERSION_MAJOR == 2 && ZIVID_CORE_VERSION_MINOR >= 2)
+// The stripe engine setting was added in SDK 2.2
+TEST_F(TestWithFileCamera, testSettingsEngine)
+{
+  waitForReady();
+  enableFirst3DAcquisition();
+  auto points_sub = subscribe<sensor_msgs::PointCloud2>(points_xyz_topic_name);
+
+  dynamic_reconfigure::Client<zivid_camera::SettingsConfig> settings_client("/zivid_camera/settings/");
+  zivid_camera::SettingsConfig settings_cfg;
+  ASSERT_TRUE(settings_client.getDefaultConfiguration(settings_cfg, dr_get_max_wait_duration));
+  ASSERT_EQ(settings_cfg.experimental_engine, zivid_camera::Settings_ExperimentalEnginePhase);
+  settings_cfg.experimental_engine = zivid_camera::Settings_ExperimentalEngineStripe;
+  settings_cfg.processing_filters_reflection_removal_enabled = true;
+  settings_cfg.processing_filters_experimental_contrast_distortion_correction_enabled = true;
+  ASSERT_TRUE(settings_client.setConfiguration(settings_cfg));
+
+  zivid_camera::Capture capture;
+  // Capture fails here because file camera does not support Stripe engine
+  ASSERT_FALSE(ros::service::call(capture_service_name, capture));
+  ASSERT_EQ(points_sub.numMessages(), 0U);
+
+  settings_cfg.experimental_engine = zivid_camera::Settings_ExperimentalEnginePhase;
+  ASSERT_TRUE(settings_client.setConfiguration(settings_cfg));
+  ASSERT_TRUE(ros::service::call(capture_service_name, capture));
+  short_wait_duration.sleep();
+  ASSERT_EQ(points_sub.numMessages(), 1U);
+}
+#endif
+
 TEST_F(ZividNodeTest, testCaptureCameraInfo)
 {
   waitForReady();

zivid_camera/test/test_zivid_camera.test

@@ -2,5 +2,6 @@
     <node name="zivid_camera" pkg="zivid_camera" type="zivid_camera_node" ns="zivid_camera" output="screen">
         <param name="file_camera_path" type="str" value="/usr/share/Zivid/data/FileCameraZividOne.zfc" />
     </node>
-    <test test-name="zivid_camera_test" pkg="zivid_camera" type="zivid_camera_test" time-limit="300.0" />
+    <test test-name="zivid_camera_test" pkg="zivid_camera" type="zivid_camera_test" time-limit="500.0"
+        launch-prefix="bash -c 'rosservice call --wait /zivid_camera/zivid_camera/set_logger_level ros.zivid_camera debug; $0 $@' " />
 </launch>

zivid_samples/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>zivid_samples</name>
-  <version>2.0.0</version>
+  <version>2.2.0</version>
   <description>Contains C++ and Python samples demonstrating
   use of the zivid_camera package.</description>
   <maintainer email="[email protected]">Zivid</maintainer>