Configuring sensor settings via the ifm3d-ros upon node startup

I'm modifying ifm3d-ros to load JSON configuration files on node startup to allow for greater visibility and control of sensor settings (such as AutoExposure) across our robot fleet. To this end, I've added the following method to the ifm3d_ros::CameraNodelet class -

bool
ifm3d_ros::CameraNodelet::ConfigureCamera(){
 std::lock_guard<std::mutex> lock(this->mutex_);
// Load appropriate JSON config file, if present
 std::string robot_name;
 if (!np_.getParam("/device_name", robot_name)) {
 ROS_ERROR_STREAM("'" << robot_name << "' param is not retrieved cannot be retrieved from the param server");
 return false;
 }
std::string cfg_path = ros::package::getPath(CONFIG_DIR)+ "/" + robot_name + "/ifm_settings";
 std::string cfg_filename = "/" + frame_id_base_ + ".json";
 std::string cfg_file_path = cfg_path + cfg_filename;
 std::ifstream config_file(cfg_file_path);
 if (config_file) {
 NODELET_INFO_STREAM("Loading configuration from file: " << cfg_path);
 json config_struct;
 try {
 config_struct = json::parse(config_file);
 ROS_WARN_STREAM("Loaded " << frame_id_base_ << " settings from " << cfg_file_path);
 } catch (const std::exception& std_ex) {
 ROS_ERROR_STREAM("Unable to load JSON");
 return false;
 }
int status = 0;
 try {
 this->cam_->FromJSON(config_struct);
 } catch (const ifm3d::error_t& ex) {
 status = ex.code();
 NODELET_ERROR_STREAM("Config error: " << ex.what());
 } catch (const std::exception& std_ex) {
 status = -1;
 NODELET_ERROR_STREAM("Config error: " << std_ex.what());
 } catch (...) {
 status = -2;
 NODELET_ERROR_STREAM("Config error: Unknown error in Config");
 }
 if (status != 0) {
 return false;
 }
 } else {
NODELET_INFO_STREAM("Config file missing. Current settings will be dumped in " << cfg_file_path);
//If RSC directory doesn't exist, create it before creating the IFM settings directory
 std::string rsc_dir_path = ros::package::getPath(CONFIG_DIR)+ "/" + robot_name;
 if (!createDirectory(rsc_dir_path)) {
 NODELET_INFO_STREAM("Error creating ifm settings directory: " << rsc_dir_path);
 return false;
 }

 if (!createDirectory(cfg_path)) {
 NODELET_INFO_STREAM("Error creating ifm settings directory: " << cfg_path);
 return false;
 }
std::string curr_config = this->cam_->ToJSONStr();
 json cfg_json = json::parse(curr_config);
std::ofstream of_str(cfg_path + cfg_filename);
 of_str << std::setw(4) << cfg_json << std::endl;
 }
 return true;
}

The critical part of this method is its usage of the following function

ifm3d::Camera::FromJSON(const json& j)

I call this function right after InitStructures() in the main Run() method of the ifm3d_ros::CameraNodelet class

Consider the following JSON config -

{
  "ifm3d": {
    "Apps": [
      {
        "Description": "",
        "Id": "476707713",
        "Imager": {
          "AutoExposureMaxExposureTime": "10000",
          "AutoExposureMaxNumberOfSaturatedPixels": "20",
          "AutoExposureReferencePointX": "88",
          "AutoExposureReferencePointY": "66",
          "AutoExposureReferenceROI": "{\"ROIs\":[
{\"id\":0,\"group\":0,\"type\":\"Rect\",\"width\":224,\"height\":172,\"angle\":0,\"center_x\":112,\"center_y\":86}
]}",
          "AutoExposureReferenceType": "0",
          "ContinuousAutoExposure": "true",
          "ExposureTime": "1120",
          "ExposureTimeRatio": "20",
          "FrameRate": "5",
          "MaxAllowedFrameRate": "5.20833333333333",
          "MinimumAmplitude": "15",
          "SpatialFilter":
{            "MaskSize": "0"          }
,
          "SpatialFilterType": "1",
          "SymmetryThreshold": "100",
          "TemporalFilter":
{            "": ""          }
,
          "TemporalFilterType": "0",
          "Type": "upTo07m_moderate"
        },
        "Index": "1",
        "Name": "",
        "OutputAmplitudeImage": "true",
        "OutputConfidenceImage": "true",
        "OutputDistanceImage": "true",
        "OutputGrayscaleImage": "false",
        "OutputXYZImage": "false",
        "TriggerMode": "1",
        "Type": "Camera"
      }
    ],
    "Device":
{      "ArticleNumber": "O3X100",      "ArticleStatus": "AC",      "Description": "",      "DeviceType": "1:512",      "IPAddressConfig": "0",      "ImageTimestampReference": "1516805626",      "Name": "New sensor",      "OperatingMode": "0",      "PasswordActivated": "false",      "SessionTimeout": "30",      "TemperatureIllu": "34.5625",      "UpTime": "0.625277777777778"    }
,
    "Net":
{      "MACAddress": "00:02:01:40:E7:1B",      "NetworkSpeed": "0",      "StaticIPv4Address": "192.168.0.71",      "StaticIPv4Gateway": "192.168.0.1",      "StaticIPv4SubNetMask": "255.255.255.0",      "UseDHCP": "false"    }
,
    "Time":
{      "CurrentTime": "1516805625",      "NTPServers": "",      "StartingSynchronization": "false",      "Stats": "",      "SynchronizationActivated": "false",      "Syncing": "false",      "WaitSyncTries": "2"    }
,
    "_": {
      "Date": "Mon Sep  9 14:54:54 2019",
      "HWInfo":
{        "MACAddress": "00:02:01:40:E7:1B",        "Mainboard": "#!02_M162_C10_03885116_008026861",        "MiraSerial": "Not implemented"      }
,
      "SWVersion":
{        "Algorithm_Version": "0.2.0",        "Calibration_Device": "00:02:01:40:e7:1b",        "Calibration_Version": "1.0.2",        "ELDK": "GOLDENEYE_YOCTO_HARDFP-371-06d9c894636352a6c93711c7284d02b0c794a527",        "IFM_Software": "1.0.126",        "Linux": "Linux version 3.14.34-rt31-yocto-standard-00029-gdf05c54-dirty (jenkins@dettlx152) (gcc version 4.9.2 (GCC) ) #1 SMP PREEMPT RT Wed Jan 24 13:40:11 CET 2018",        "Main_Application": "0.4.1483"      }
,
      "ifm3d_version": 1102
    }
  }
 }

I am now able to configure different values for params such as ContinuousAutoExposure, SymmetryThreshold etc. However, when I try to adjust the values for "OutputXYZImage", they do not change on the sensor. Here's a diff snippet -

diff is_mid_set.json ifm_front_mid.json 
 18c18
 <           "MaxAllowedFrameRate": "5.20833333333333",
 —
 >           "MaxAllowedFrameRate": "5.5",
 37c37
 <         "OutputXYZImage": "true",
 —
 >         "OutputXYZImage": "false",
 48c48
 <       "ImageTimestampReference": "1516808883",
 —
 >       "ImageTimestampReference": "1516811098",
 53,54c53,54
 <       "TemperatureIllu": "34.875",
 <       "UpTime": "1.53027777777778"
 —
 >       "TemperatureIllu": "37.4375",
 >       "UpTime": "2.14555555555556"
 65c65
 <       "CurrentTime": "1516808883",
 —
 >       "CurrentTime": "1516811098",
 74c74
 <       "Date": "Mon Sep  9 14:49:12 2019",
 —
 >       "Date": "Thu Sep  5 14:53:55 2019",

If I pass the same JSON to the command line tool ifm3d dump, the values for those items do indeed change. Should I be doing something different in my method to set these values via the camera nodelet?

Please let me know if you need additional information. Thanks!

ifm / ifm3d-ros

Configuring sensor settings via the ifm3d-ros upon node startup #23