Add param to change filter levels (#20)

This feature was added in royale 3.21, see:

https://pmdtec.com/picofamily/2019/02/22/royale_release_v321/

The new filter level "Full" includes some additional filters (i.e.
smoothing, hole filling etc.) that improve the depth image smoothness.

Author: mintar

CMakeLists.txt

@@ -52,6 +52,7 @@ string(REPLACE " " ";" cxx_flags "${CMAKE_CXX_FLAGS} ${additional_cxx_flags}")
 MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS})
 MESSAGE( STATUS "additional_cxx_flags: " ${additional_cxx_flags})
 MESSAGE( STATUS "cxx_flags: ${cxx_flags}")
+MESSAGE( STATUS "royale version: ${royale_VERSION}")
 
 
 ################################################
@@ -102,6 +103,7 @@ target_link_libraries(pico_flexx_nodelet
 )
 add_dependencies(pico_flexx_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_compile_options(pico_flexx_nodelet PUBLIC ${cxx_flags})
+target_compile_definitions(pico_flexx_nodelet PUBLIC royale_VERSION_MAJOR=${royale_VERSION_MAJOR} royale_VERSION_MINOR=${royale_VERSION_MINOR})
 
 add_executable(pico_flexx_driver src/pico_flexx_driver.cpp)
 target_link_libraries(pico_flexx_driver
@@ -110,6 +112,7 @@ target_link_libraries(pico_flexx_driver
 )
 add_dependencies(pico_flexx_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_compile_options(pico_flexx_driver PUBLIC ${cxx_flags})
+target_compile_definitions(pico_flexx_driver PUBLIC royale_VERSION_MAJOR=${royale_VERSION_MAJOR} royale_VERSION_MINOR=${royale_VERSION_MINOR})
 
 
 

README.md

@@ -31,7 +31,7 @@ Features:
 ## Dependencies
 
 - ROS Indigo or Kinetic (or newer should also work)
-- [Royale SDK](http://www.pmdtec.com/picoflexx/)
+- [Royale SDK](http://www.pmdtec.com/picoflexx/) (recommended: latest version, at least 3.21)
 
 ## Status
 

cfg/pico_flexx_driver.cfg

@@ -21,12 +21,21 @@ exposure_modes = gen.enum([ gen.const("MANUAL", int_t, 0, "Manual exposure mode"
                             gen.const("AUTOMATIC", int_t, 1, "Automatic exposure mode")
                           ], "Possible exposure modes")
 
+# Royale allows to set different filter levels. Internally these represent
+# different configurations of the processing pipeline.
+filter_levels = gen.enum([ gen.const("Off", int_t, 0, "Turn off all filtering of the data (validation will still be enabled)"),
+                           gen.const("Legacy", int_t, 200, "Standard settings for older cameras"),
+                           gen.const("Full", int_t, 255, "Enable all filters that are available for this camera"),
+                           gen.const("Custom", int_t, 256, "Value returned by getFilterLevel if the processing parameters differ from all of the presets")
+                          ], "Possible filter levels")
+
 gen.add("use_case",               int_t,    0x01, "Use cases for the sensor", 0, 0, 9, edit_method=use_cases)
 gen.add("exposure_mode",          int_t,    0x02, "Exposure mode for the sensor", 0, 0, 1, edit_method=exposure_modes)
 gen.add("exposure_mode_stream2",  int_t,    0x04, "Exposure mode for the sensor (stream 2)", 0, 0, 1, edit_method=exposure_modes)
 gen.add("exposure_time",          int_t,    0x08, "Exposure time", 1000, 1, 2000)
 gen.add("exposure_time_stream2",  int_t,    0x10, "Exposure time (stream 2)", 1000, 1, 2000)
 gen.add("max_noise",              double_t, 0x20, "Max allowed noise", 0.07, 0.0, 0.1)
 gen.add("range_factor",           double_t, 0x40, "Range of factor times standard deviation arround mean", 2.0, 0.0, 7.0)
+gen.add("filter_level",           int_t,    0x80, "Filter level", 200, 0, 256, edit_method=filter_levels)
 
 exit(gen.generate(PACKAGE, "pico_flexx_driver", "pico_flexx_driver"))

launch/pico_flexx_driver.launch

@@ -17,6 +17,8 @@
   <arg name="exposure_mode_stream2"  default="1"/>
   <!-- Maximum allowed noise. Data with higher noise will be filtered out. -->
   <arg name="max_noise"          default="0.07"/>
+  <!-- Filter level (0 = Off, 200 = Legacy, 255 = Full) -->
+  <arg name="filter_level"       default="200"/>
   <!-- Range of the 16-Bit mono image which should be mapped to the 0-255 range of the 8-Bit mono image. The resulting range is `range_factor` times the standard deviation arround mean. -->
   <arg name="range_factor"       default="2.0"/>
   <!-- Queue size for publisher. -->
@@ -58,6 +60,7 @@
     <param name="exposure_time_stream2" type="int"    value="$(arg exposure_time_stream2)"/>
     <param name="exposure_mode_stream2" type="int"    value="$(arg exposure_mode_stream2)"/>
     <param name="max_noise"          type="double" value="$(arg max_noise)"/>
+    <param name="filter_level"       type="int"    value="$(arg filter_level)"/>
     <param name="range_factor"       type="double" value="$(arg range_factor)"/>
     <param name="queue_size"         type="int"    value="$(arg queue_size)"/>
     <param name="base_name_tf"       type="str"    value="$(arg base_name_tf)"/>

src/pico_flexx_driver.cpp

@@ -420,6 +420,18 @@ class PicoFlexx : public royale::IDepthDataListener, public royale::IExposureLis
       lockStatus.unlock();
     }
 
+    if(level & (0x01 | 0x80))
+    {
+      // setting use case resets filter level, so we have to do it again here
+      OUT_INFO("reconfigured filter_level: " << FG_CYAN << config.filter_level << NO_COLOR);
+      if (setFilterLevel(config.filter_level))
+      {
+        this->config.filter_level = config.filter_level;
+      } else {
+        config.filter_level = this->config.filter_level;
+      }
+    }
+
     if(level & (0x01 | 0x02 | 0x04))
     {
       royale::Pair<uint32_t, uint32_t> limits;
@@ -849,6 +861,48 @@ class PicoFlexx : public royale::IDepthDataListener, public royale::IExposureLis
     return true;
   }
 
+  bool setFilterLevel(const int level, const royale::StreamId streamId = 0)
+  {
+#if (defined(royale_VERSION_MAJOR) && defined(royale_VERSION_MINOR) \
+     && (royale_VERSION_MAJOR > 3 || (royale_VERSION_MAJOR == 3 && royale_VERSION_MINOR >= 21)))
+    const royale::FilterLevel desiredLevel = (royale::FilterLevel) level;
+    royale::FilterLevel currentLevel;
+    if (cameraDevice->getFilterLevel(currentLevel) != royale::CameraStatus::SUCCESS)
+    {
+      OUT_ERROR("could not get filter level!");
+      return false;
+    }
+    OUT_INFO("current filter level: " << FG_CYAN << (int) currentLevel << NO_COLOR);
+    this->config.filter_level = (int) currentLevel;
+
+    if (desiredLevel == currentLevel)
+    {
+      OUT_INFO("filter level unchanged");
+      return false;
+    }
+    if (desiredLevel == royale::FilterLevel::Custom)
+    {
+      OUT_INFO("filter level 'Custom' can only be read, not set");
+      return false;
+    }
+    if (currentLevel == royale::FilterLevel::Custom)
+    {
+      OUT_INFO("current filter level is 'Custom', will not overwrite");
+      return false;
+    }
+    if (cameraDevice->setFilterLevel(desiredLevel, streamId) != royale::CameraStatus::SUCCESS)
+    {
+      OUT_ERROR("could not set filter level!");
+      return false;
+    }
+    OUT_INFO("filter level changed to: " << FG_YELLOW << (int) desiredLevel);
+    return true;
+#else
+    OUT_ERROR("setting the filter level requires royale >= 3.21!");
+    return false;
+#endif
+  }
+
   bool createCameraInfo(const royale::LensParameters &params)
   {
     if(params.distortionRadial.size() != 3)