minor modifications. after verif. with ros and opencv

CMakeLists.txt

@@ -7,6 +7,9 @@ find_package(catkin REQUIRED)
 #                 Add catkin additional components here
 # ******************************************************************** 
 find_package(catkin REQUIRED COMPONENTS iri_base_driver)
+find_package(mvbluefox3 REQUIRED)
+find_package(mvIMPACT REQUIRED)
+# FIND_PACKAGE(OpenCV 3)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -64,7 +67,8 @@ catkin_package(
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 
-#  DEPENDS 
+ DEPENDS mvbluefox3 mvIMPACT
+ # DEPENDS mvbluefox3 mvIMPACT OpenCV
 )
 
 ###########
@@ -76,6 +80,9 @@ catkin_package(
 # ******************************************************************** 
 include_directories(include)
 include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${mvbluefox3_INCLUDE_DIR})
+include_directories(${mvIMPACT_INCLUDE_DIR})
+# INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
 # include_directories(${<dependency>_INCLUDE_DIR})
 
 ## Declare a cpp library
@@ -88,6 +95,9 @@ add_executable(${PROJECT_NAME} src/mvbluefox3_camera_driver.cpp src/mvbluefox3_c
 #                   Add the libraries
 # ******************************************************************** 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME} ${mvbluefox3_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${mvIMPACT_LIBRARY})
+# TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
 # target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
 
 # ******************************************************************** 

include/mvbluefox3_camera_driver.h

@@ -28,7 +28,7 @@
 #include <iri_base_driver/iri_base_driver.h>
 #include <iri_mvbluefox3_camera/Mvbluefox3CameraConfig.h>
 
-//include mvbluefox3_camera_driver main library
+#include "mvbluefox3.h"
 
 /**
  * \brief IRI ROS Specific Driver Class
@@ -54,6 +54,41 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
   private:
     // private attributes and methods
 
+    CMvbluefox3::CMvbluefox3* cam_ptr;   // Camera pointer. 
+
+    std::string serial; // Camera serial number.
+
+    CMvbluefox3::CParams params; // Camera parameters.
+
+    /**
+     * \brief Delete pointer
+     *
+     *  Function to delete pointers.
+     */
+    template <typename Ptr_t>
+    void DelPtr(Ptr_t &ptr) 
+    {
+      if (ptr != NULL) 
+      {
+        ptr = NULL;
+        delete [] ptr;
+      }
+    }
+
+   /**
+    * \brief Set Configuration
+    *
+    * Set Configuration with params_ object (to be filled externally).
+    */    
+    void SetConfig(void);  
+
+   /**
+    * \brief Get Configuration
+    *
+    * Get Configuration.
+    */  
+    void GetConfig(void);
+
   public:
    /**
     * \brief define config type
@@ -142,8 +177,33 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
     */
     void config_update(Config& new_cfg, uint32_t level=0);
 
-    // here define all mvbluefox3_camera_driver interface methods to retrieve and set
-    // the driver parameters
+   /**
+    * \brief Get image channel's depth
+    *
+    * Get image channel's depth.
+    */  
+    int GetChDepth(void);
+
+   /**
+    * \brief Get bytes-pixel
+    *
+    * Get bytes-pixel.
+    */ 
+    int GetBytesXPixel(void);
+
+   /**
+    * \brief Get image
+    *
+    * Get image.
+    */ 
+    void GetImage(char *image);
+
+   /**
+    * \brief Get real exposure time
+    *
+    * Get real exposure time.
+    */ 
+    int ReqExposeUs(void);
 
    /**
     * \brief Destructor

include/mvbluefox3_camera_driver_node.h

@@ -54,6 +54,7 @@
 class Mvbluefox3CameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Mvbluefox3CameraDriver>
 {
   private:
+
     // [publisher attributes]
 
     // [subscriber attributes]

src/mvbluefox3_camera_driver.cpp

 #include "mvbluefox3_camera_driver.h"
 
+// #include <opencv2/opencv.hpp>
+// #define WINDOW_NAME "mvBlueFOX3 TEST"
+
 Mvbluefox3CameraDriver::Mvbluefox3CameraDriver(void)
 {
-  //setDriverId(driver string id);
+  this->cam_ptr = NULL;
+
+  // TMP: To be substituted by config_update
+  this->serial = "F0300141";
 }
 
-bool Mvbluefox3CameraDriver::openDriver(void)
+Mvbluefox3CameraDriver::~Mvbluefox3CameraDriver(void)
 {
-  //setDriverId(driver string id);
+  DelPtr(this->cam_ptr);
+}
 
+bool Mvbluefox3CameraDriver::openDriver(void)
+{
+  std::cout << "open" << std::endl;
+  try 
+  {
+    this->cam_ptr = new CMvbluefox3::CMvbluefox3(this->serial);
+  }catch (CMvbluefox3::CmvBlueFOX3Exception& e) 
+  {
+    std::cout << e.what() << std::endl;
+  }    
   return true;
 }
 
@@ -19,6 +36,21 @@ bool Mvbluefox3CameraDriver::closeDriver(void)
 
 bool Mvbluefox3CameraDriver::startDriver(void)
 {
+  // std::cout << "[Camera test]: Acquiring images." << std::endl;
+
+  // cv::namedWindow( WINDOW_NAME, cv::WINDOW_NORMAL );
+    
+  // while (true) 
+  // {
+  //   cv::Mat image;
+  //   this->cam_ptr->GetImageCV(image);
+  //   cv::imshow( WINDOW_NAME, image );
+  //   if(cv::waitKey(30) >= 0) break;
+  // }
+  // cv::destroyAllWindows();
+
+  std::cout << "[ROS mvBlueFOX3]: Driver started." << std::endl;
+
   return true;
 }
 
@@ -35,13 +67,29 @@ void Mvbluefox3CameraDriver::config_update(Config& new_cfg, uint32_t level)
   // update driver with new_cfg data
   switch(this->getState())
   {
-    case Mvbluefox3CameraDriver::CLOSED:
+    case Mvbluefox3CameraDriver::CLOSED: 
       break;
 
     case Mvbluefox3CameraDriver::OPENED:
+      this->lock();
+      // Fill parameters
+      // this->serial = new_cfg.serial;
+      // this->dyn_rec_to_camera(new_cfg,this->desired_conf);
+      // update the frame identifier
+      // this->frame_id = new_cfg.frame_id;
+      SetConfig();
+      this->unlock();      
       break;
 
     case Mvbluefox3CameraDriver::RUNNING:
+      this->lock();
+      // Fill parameters
+      // this->serial = new_cfg.serial;
+      // this->dyn_rec_to_camera(new_cfg,this->desired_conf);
+      // update the frame identifier
+      // this->frame_id = new_cfg.frame_id;
+      SetConfig();
+      this->unlock();    
       break;
   }
 
@@ -51,6 +99,33 @@ void Mvbluefox3CameraDriver::config_update(Config& new_cfg, uint32_t level)
   this->unlock();
 }
 
-Mvbluefox3CameraDriver::~Mvbluefox3CameraDriver(void)
+void Mvbluefox3CameraDriver::SetConfig(void)
 {
+  this->cam_ptr->SetConfig(this->params);
 }
+
+void Mvbluefox3CameraDriver::GetConfig(void)
+{
+  this->params = this->cam_ptr->GetConfig();
+}
+
+int Mvbluefox3CameraDriver::GetChDepth(void)
+{
+  return this->cam_ptr->GetChDepth();
+}
+
+int Mvbluefox3CameraDriver::GetBytesXPixel(void)
+{
+  return this->cam_ptr->GetBytesXPixel();
+}
+
+void Mvbluefox3CameraDriver::GetImage(char *image)
+{
+  this->cam_ptr->GetImage(&image);
+}
+
+int Mvbluefox3CameraDriver::ReqExposeUs(void)
+{
+  return this->cam_ptr->ReqExposeUs();
+}
+