From b87ffd644b48b8eb5558254bab3dbac78b60fc81 Mon Sep 17 00:00:00 2001
From: asantamaria <asantamaria@iri.upc.edu>
Date: Tue, 13 Dec 2016 10:12:58 +0100
Subject: [PATCH] minor modifications. after verif. with ros and opencv
---
CMakeLists.txt | 12 +++-
include/mvbluefox3_camera_driver.h | 66 ++++++++++++++++++-
include/mvbluefox3_camera_driver_node.h | 1 +
src/mvbluefox3_camera_driver.cpp | 85 +++++++++++++++++++++++--
4 files changed, 155 insertions(+), 9 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95682a5..7624e98 100644
--- a/CMakeLists.txt
+++ b/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})
# ********************************************************************
diff --git a/include/mvbluefox3_camera_driver.h b/include/mvbluefox3_camera_driver.h
index e3d31dd..7465a46 100644
--- a/include/mvbluefox3_camera_driver.h
+++ b/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
diff --git a/include/mvbluefox3_camera_driver_node.h b/include/mvbluefox3_camera_driver_node.h
index 9ba071c..4ecdfdb 100644
--- a/include/mvbluefox3_camera_driver_node.h
+++ b/include/mvbluefox3_camera_driver_node.h
@@ -54,6 +54,7 @@
class Mvbluefox3CameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Mvbluefox3CameraDriver>
{
private:
+
// [publisher attributes]
// [subscriber attributes]
diff --git a/src/mvbluefox3_camera_driver.cpp b/src/mvbluefox3_camera_driver.cpp
index 780f2e4..362420c 100644
--- a/src/mvbluefox3_camera_driver.cpp
+++ b/src/mvbluefox3_camera_driver.cpp
@@ -1,14 +1,31 @@
#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();
+}
+
--
GitLab