filter_level is not initialized on the startup of sensor

[aravindsairam]

The driver is really helpful and makes it easy to control the pico flexx sensors. Thanks for this package.

The parameter filter_level is mentioned in the launch file but not really initialized on the startup.

For example, by default this parameter filter_level is set to 200 (Legacy) but not really used during the launch of the sensor. This works with dynamic reconfigure but not during the start of the sensor. So when the user set the value of the filter_level in the launch file, it did not really change/initialize the filter_level.

I came up initializing this on the startup and added few lines in the code.

  priv_nh.param("base_name", baseName, std::string(PF_DEFAULT_NS));
  priv_nh.param("sensor", sensor, std::string(""));
  priv_nh.param("use_case", useCase, 0);
  priv_nh.param("automatic_exposure", automaticExposure, true);
  priv_nh.param("automatic_exposure", automaticExposureStream2, true);
  priv_nh.param("exposure_time", exposureTime, 1000);
  priv_nh.param("exposure_time_stream2", exposureTimeStream2, 1000);
  priv_nh.param("max_noise", maxNoise, 0.7);
  priv_nh.param("range_factor", rangeFactor, 2.0);
  priv_nh.param("queue_size", queueSize, 2);
  priv_nh.param("base_name_tf", baseNameTF, baseName);
  priv_nh.param("filter_level", filterLevel, 200); // a node handler param for filter_level 

OUT_INFO("parameter:" << std::endl
         << "                 base_name: " FG_CYAN << baseName << NO_COLOR << std::endl
         << "                    sensor: " FG_CYAN << (sensor.empty() ? "default" : sensor) << NO_COLOR << std::endl
         << "                  use_case: " FG_CYAN << useCase << NO_COLOR << std::endl
         << "        automatic_exposure: " FG_CYAN << (automaticExposure ? "true" : "false") << NO_COLOR << std::endl
         << "automatic_exposure_stream2: " FG_CYAN << (automaticExposureStream2 ? "true" : "false") << NO_COLOR << std::endl
         << "             exposure_time: " FG_CYAN << exposureTime << NO_COLOR << std::endl
         << "     exposure_time_stream2: " FG_CYAN << exposureTimeStream2 << NO_COLOR << std::endl
         << "                 max_noise: " FG_CYAN << maxNoise << " meters" NO_COLOR << std::endl
         << "              range_factor: " FG_CYAN << rangeFactor << NO_COLOR << std::endl
         << "                queue_size: " FG_CYAN << queueSize << NO_COLOR << std::endl
         << "              base_name_tf: " FG_CYAN << baseNameTF << NO_COLOR << std::endl
         << "              filter_level: " FG_CYAN << filterLevel << NO_COLOR); // print this info of filter level 

uint32_t major, minor, patch, build;
royale::getVersion(major, minor, patch, build);
OUT_INFO("libroyale version: " FG_CYAN << major << '.' << minor << '.' << patch << '.' << build << NO_COLOR);

royale::LensParameters params;
if(!selectCamera(sensor)
   || !setUseCase((size_t)useCase)
   || !setExposureModeAllStreams(automaticExposure, automaticExposureStream2)
   || !getCameraSettings(params)
   || !createCameraInfo(params)
   || !setFilterLevel(filterLevel)) // set filter_level on the startup and check 
{
  return false;
}`