From a6a0c7e87c904055286a7a2be5d8ca605da30e1f Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Mon, 1 Apr 2019 12:29:56 +0200
Subject: [PATCH] Add load_cfg param. Incrase frame_rate param max value.
 Comment GetConfig for every get image

---
 cfg/IriMvbluefox3CameraDriver.cfg         | 3 ++-
 src/iri_mvbluefox3_camera_driver_alg.cpp  | 5 +++--
 src/iri_mvbluefox3_camera_driver_node.cpp | 4 +++-
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/cfg/IriMvbluefox3CameraDriver.cfg b/cfg/IriMvbluefox3CameraDriver.cfg
index a91f2a6..1c15f06 100755
--- a/cfg/IriMvbluefox3CameraDriver.cfg
+++ b/cfg/IriMvbluefox3CameraDriver.cfg
@@ -67,12 +67,13 @@ gen.const("BGR8", int_t, 5, "BGR coding with 8 bits per channel"),
 gen.const("YUV422", int_t, 6, "YUV coding with 4,2,2 pixels per channel"),
 gen.const("YUV444", int_t, 7, "YUV coding with 4,4,4 pixels per channel")],"Available color modes")
 
+gen.add("load_cfg",                  bool_t, 0, "Load current configuration",                                      False)
 gen.add("cam_name",                  str_t,  0, "Camera number to apply settings",                                 "<< FILL CAMERA NAME TO UPDATE  PARAMS>>")
 gen.add("apply_cfg",                 bool_t, 0, "Apply new configuration",                                         False)
 gen.add("pixel_format",              int_t,  0, "Color modes",                                                     1,      0,     7, edit_method=enum_color_coding)
 gen.add("width",                     int_t,  0, "Image width",                                                     1280,   0,     1280)
 gen.add("height",                    int_t,  0, "Image height",                                                    960,    0,     960)
-gen.add("frame_rate",                int_t,  0, "Frame rate",                                                      1,      1,     29)
+gen.add("frame_rate",                int_t,  0, "Frame rate",                                                      1,      1,     100000)
 gen.add("pixel_clock",               int_t,  0, "Pixel Clock (kHz)",                                               40000,  40000, 66000, edit_method=enum_clock)
 gen.add("auto_expose",               bool_t, 0, "Enable automatic exposure control",                               True)
 gen.add("auto_expose_upperlimit",    int_t,  0, "Auto exposure upper limit [us]",                                  30000,  10000, 500000)
diff --git a/src/iri_mvbluefox3_camera_driver_alg.cpp b/src/iri_mvbluefox3_camera_driver_alg.cpp
index 55d9e44..98f13fb 100644
--- a/src/iri_mvbluefox3_camera_driver_alg.cpp
+++ b/src/iri_mvbluefox3_camera_driver_alg.cpp
@@ -128,8 +128,9 @@ iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig IriMvbluefox3Camer
 {
   iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig cfg;
 
-  cfg.apply_cfg = false;
   cfg.cam_name = this->vcam_name[cam_num];
+  cfg.load_cfg = false;
+  cfg.apply_cfg = false;
   cfg.pixel_format = CodesToPf(p.pixel_format);
   cfg.width = p.width;
   cfg.height = p.height;
@@ -225,7 +226,7 @@ void IriMvbluefox3CameraDriverAlgorithm::GetROSImage(const int &ncam, sensor_msg
 
   char *image = NULL;
   this->vcam_ptr[ncam]->GetImage(&image);
-  GetConfig(ncam);
+  //GetConfig(ncam);
 
   std::string encoding;
   encoding = CMvbluefox3::codings_str[this->vparams[ncam].pixel_format];
diff --git a/src/iri_mvbluefox3_camera_driver_node.cpp b/src/iri_mvbluefox3_camera_driver_node.cpp
index d8e1c38..caf0c1e 100644
--- a/src/iri_mvbluefox3_camera_driver_node.cpp
+++ b/src/iri_mvbluefox3_camera_driver_node.cpp
@@ -218,11 +218,13 @@ void IriMvbluefox3CameraDriverNode::DynRecCallback(Config &config, uint32_t leve
 
 
     // Initialize dynamic reconfigure with first camera_driver
-    if (this->alg_.ini_dynrec && this->alg_.num_cams > 0)
+    if ((this->alg_.ini_dynrec && this->alg_.num_cams > 0 ) || config.load_cfg)
     {
+      ROS_INFO("IriMvbluefox3CameraDriverNode::DynRecCallback: loading current config");
       this->alg_.GetConfig(0);
       config = this->alg_.ParamsToDynRec(0,this->alg_.vparams[0]);
       this->alg_.ini_dynrec = false;
+      config.load_cfg=false;
     }
 
     // Set new config is requested
-- 
GitLab