From 5d2caadc0d8a0dbbc24a73eed0c3d3e8007c51c5 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Mon, 1 Apr 2019 12:16:33 +0200
Subject: [PATCH] Clean indentation, from tabs to double space
---
include/iri_mvbluefox3_camera_driver_alg.h | 366 +++++++--------
include/iri_mvbluefox3_camera_driver_node.h | 146 +++---
src/iri_mvbluefox3_camera_driver_alg.cpp | 492 ++++++++++----------
src/iri_mvbluefox3_camera_driver_node.cpp | 442 +++++++++---------
4 files changed, 723 insertions(+), 723 deletions(-)
diff --git a/include/iri_mvbluefox3_camera_driver_alg.h b/include/iri_mvbluefox3_camera_driver_alg.h
index 21e8e2b..4033938 100644
--- a/include/iri_mvbluefox3_camera_driver_alg.h
+++ b/include/iri_mvbluefox3_camera_driver_alg.h
@@ -46,52 +46,52 @@ class CMutex
{
protected:
- /**
- * \brief define config type
- *
- * Define a Config type with the IriMvbluefox3CameraDriverConfig. All driver implementations
- * will then use the same variable type Config.
- */
- pthread_mutex_t access_;
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the IriMvbluefox3CameraDriverConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
public:
- CMutex()
+ CMutex()
{
- pthread_mutex_init(&this->access_,NULL);
+ pthread_mutex_init(&this->access_,NULL);
};
- ~CMutex()
- {
- pthread_mutex_destroy(&this->access_);
- };
- /**
- * \brief Lock Algorithm
- *
- * Locks access to the Algorithm class
- */
- void lock(void) { pthread_mutex_lock(&this->access_); };
-
- /**
- * \brief Unlock Algorithm
- *
- * Unlocks access to the Algorithm class
- */
- void unlock(void) { pthread_mutex_unlock(&this->access_); };
-
- /**
- * \brief Tries Access to Algorithm
- *
- * Tries access to Algorithm
- *
- * \return true if the lock was adquired, false otherwise
- */
- bool try_enter(void)
- {
- if(pthread_mutex_trylock(&this->access_)==0)
- return true;
- else
- return false;
- };
+ ~CMutex()
+ {
+ pthread_mutex_destroy(&this->access_);
+ };
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
};
@@ -104,154 +104,154 @@ class IriMvbluefox3CameraDriverAlgorithm
{
protected:
- // private attributes and methods
+ // private attributes and methods
- CMutex alg_mutex_;
+ CMutex alg_mutex_;
- mvIMPACT::acquire::DeviceManager devMgr; // Device Manager.
+ mvIMPACT::acquire::DeviceManager devMgr; // Device Manager.
- std::vector<CMvbluefox3::CMvbluefox3*> vcam_ptr; // CameraDriver pointer.
+ std::vector<CMvbluefox3::CMvbluefox3*> vcam_ptr; // CameraDriver pointer.
public:
- int num_cams; // Number of cameras.
- std::vector<std::string> vserial; // Serial numbers.
- std::vector<std::string> vframe_id; // Frame ids.
- std::vector<std::string> vcam_name; // CameraDriver names.
- std::vector<std::string> vparams_path; // Parameters' paths.
-
- std::vector<CMvbluefox3::CParams> vparams; // CameraDriver parameters.
- bool ini_user_params; // To control parameters initialization after device is found.
-
- bool ini_dynrec; // To control parameters initialization after device is found.
-
- /**
- * \brief config variable
- *
- * This variable has all the driver parameters defined in the cfg config file.
- * Is updated everytime function config_update() is called.
- */
- Config config_;
-
- /**
- * \brief constructor
- *
- * In this constructor parameters related to the specific driver can be
- * initalized. Those parameters can be also set in the openDriver() function.
- */
- IriMvbluefox3CameraDriverAlgorithm(void);
-
- /**
- * \brief Destructor
- *
- * This destructor is called when the object is about to be destroyed.
- *
- */
- ~IriMvbluefox3CameraDriverAlgorithm(void);
-
- /**
- * \brief Lock Algorithm
- *
- * Locks access to the Algorithm class
- */
- void lock(void) { this->alg_mutex_.lock(); };
-
- /**
- * \brief Unlock Algorithm
- *
- * Unlocks access to the Algorithm class
- */
- void unlock(void) { this->alg_mutex_.unlock(); };
-
- /**
- * \brief Tries Access to Algorithm
- *
- * Tries access to Algorithm
- *
- * \return true if the lock was adquired, false otherwise
- */
- bool try_enter(void) { return this->alg_mutex_.try_enter(); };
-
- // here define all iri_mvbluefox3_camera_driver_alg interface methods to retrieve and set
- // the driver parameters
-
- /**
- * \brief Open CameraDriver driver
- */
- bool OpenDriver();
-
- /**
- * \brief Get image
- *
- * Get image.
- */
- // void GetImage(char *image);
- void GetROSImage(const int &ncam, sensor_msgs::Image &img_msg);
-
- /**
- * \brief Resize objects in driver class
- *
- * Resize objects in driver class.
- *
- */
- void ResizeObjs(void);
-
- /**
- * \brief Set Configuration
- *
- * Set Configuration with vparams_ object (to be filled externally).
- */
- void SetConfig(const int &ncam);
-
- /**
- * \brief Get Configuration
- *
- * Get Configuration.
- */
- void GetConfig(const int &ncam);
-
- /**
- * \brief Conversion from pixel format to codings_t
- *
- * Conversion from pixel format to codings_t.
- */
- CMvbluefox3::codings_t PfToCodes(int &pf);
-
- /**
- * \brief Conversion from codings_t to pixel format
- *
- * Conversion from codings_t to pixel format.
- */
- int CodesToPf(CMvbluefox3::codings_t ct);
-
- /**
- * \brief Delete pointer
- *
- * Function to delete pointers.
- */
- template <typename Ptr_t>
- void DelPtr(Ptr_t &ptr)
- {
- if (ptr != NULL)
- {
- ptr = NULL;
- delete [] ptr;
- }
- }
-
- /**
- * \brief Dynamic reconfigure to camera parameters
- *
- * Dynamic reconfigure to camera parameters.
- */
- CMvbluefox3::CParams DynRecToParams(iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig &cfg);
-
- /**
- * \brief CameraDriver parameters to dynamic reconfigure
- *
- * CameraDriver parameters to dynamic reconfigure.
- */
- iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig ParamsToDynRec(const int &cam_num, const CMvbluefox3::CParams &p);
+ int num_cams; // Number of cameras.
+ std::vector<std::string> vserial; // Serial numbers.
+ std::vector<std::string> vframe_id; // Frame ids.
+ std::vector<std::string> vcam_name; // CameraDriver names.
+ std::vector<std::string> vparams_path; // Parameters' paths.
+
+ std::vector<CMvbluefox3::CParams> vparams; // CameraDriver parameters.
+ bool ini_user_params; // To control parameters initialization after device is found.
+
+ bool ini_dynrec; // To control parameters initialization after device is found.
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ */
+ IriMvbluefox3CameraDriverAlgorithm(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~IriMvbluefox3CameraDriverAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { this->alg_mutex_.lock(); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { this->alg_mutex_.unlock(); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void) { return this->alg_mutex_.try_enter(); };
+
+ // here define all iri_mvbluefox3_camera_driver_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Open CameraDriver driver
+ */
+ bool OpenDriver();
+
+ /**
+ * \brief Get image
+ *
+ * Get image.
+ */
+ // void GetImage(char *image);
+ void GetROSImage(const int &ncam, sensor_msgs::Image &img_msg);
+
+ /**
+ * \brief Resize objects in driver class
+ *
+ * Resize objects in driver class.
+ *
+ */
+ void ResizeObjs(void);
+
+ /**
+ * \brief Set Configuration
+ *
+ * Set Configuration with vparams_ object (to be filled externally).
+ */
+ void SetConfig(const int &ncam);
+
+ /**
+ * \brief Get Configuration
+ *
+ * Get Configuration.
+ */
+ void GetConfig(const int &ncam);
+
+ /**
+ * \brief Conversion from pixel format to codings_t
+ *
+ * Conversion from pixel format to codings_t.
+ */
+ CMvbluefox3::codings_t PfToCodes(int &pf);
+
+ /**
+ * \brief Conversion from codings_t to pixel format
+ *
+ * Conversion from codings_t to pixel format.
+ */
+ int CodesToPf(CMvbluefox3::codings_t ct);
+
+ /**
+ * \brief Delete pointer
+ *
+ * Function to delete pointers.
+ */
+ template <typename Ptr_t>
+ void DelPtr(Ptr_t &ptr)
+ {
+ if (ptr != NULL)
+ {
+ ptr = NULL;
+ delete [] ptr;
+ }
+ }
+
+ /**
+ * \brief Dynamic reconfigure to camera parameters
+ *
+ * Dynamic reconfigure to camera parameters.
+ */
+ CMvbluefox3::CParams DynRecToParams(iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig &cfg);
+
+ /**
+ * \brief CameraDriver parameters to dynamic reconfigure
+ *
+ * CameraDriver parameters to dynamic reconfigure.
+ */
+ iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig ParamsToDynRec(const int &cam_num, const CMvbluefox3::CParams &p);
};
diff --git a/include/iri_mvbluefox3_camera_driver_node.h b/include/iri_mvbluefox3_camera_driver_node.h
index 84e57f2..851f352 100644
--- a/include/iri_mvbluefox3_camera_driver_node.h
+++ b/include/iri_mvbluefox3_camera_driver_node.h
@@ -52,94 +52,94 @@ class IriMvbluefox3CameraDriverNode
{
private:
- bool initialized_;
+ bool initialized_;
- // [publisher attributes]
- image_transport::ImageTransport it;
+ // [publisher attributes]
+ image_transport::ImageTransport it;
- std::vector<camera_info_manager::CameraInfoManager*> camera_manager;
- std::vector<image_transport::CameraPublisher> image_publisher_;
- std::vector<sensor_msgs::Image> image_msg_;
+ std::vector<camera_info_manager::CameraInfoManager*> camera_manager;
+ std::vector<image_transport::CameraPublisher> image_publisher_;
+ std::vector<sensor_msgs::Image> image_msg_;
- /**
- * \brief Set camera parameters
- *
- * This function sets the camera parameters. The parameters must be loaded to parameter server (yaml) within a namepsace with the camera name.
- */
- void SetParams(const ros::NodeHandle &nh, const std::string &cam_name);
+ /**
+ * \brief Set camera parameters
+ *
+ * This function sets the camera parameters. The parameters must be loaded to parameter server (yaml) within a namepsace with the camera name.
+ */
+ void SetParams(const ros::NodeHandle &nh, const std::string &cam_name);
- // [subscriber attributes]
+ // [subscriber attributes]
- // [service attributes]
+ // [service attributes]
- // [client attributes]
+ // [client attributes]
- // [action server attributes]
+ // [action server attributes]
- // [action client attributes]
+ // [action client attributes]
- /**
- * \brief template algorithm class
- *
- * This template class refers to an implementation of an specific algorithm
- * interface. Will be used in the derivate class to define the common
- * behaviour for all the different implementations from the same algorithm.
- */
- IriMvbluefox3CameraDriverAlgorithm alg_;
+ /**
+ * \brief template algorithm class
+ *
+ * This template class refers to an implementation of an specific algorithm
+ * interface. Will be used in the derivate class to define the common
+ * behaviour for all the different implementations from the same algorithm.
+ */
+ IriMvbluefox3CameraDriverAlgorithm alg_;
- /**
- * \brief config variable
- *
- * This variable has all the driver parameters defined in the cfg config file.
- * Is updated everytime function config_update() is called.
- */
- Config config_;
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
public:
- /**
- * \brief Constructor
- *
- * This constructor initializes specific class attributes and all ROS
- * communications variables to enable message exchange.
- */
- IriMvbluefox3CameraDriverNode(ros::NodeHandle &nh, dynamic_reconfigure::Server<Config> &dsrv);
-
- /**
- * \brief Destructor
- *
- * This destructor frees all necessary dynamic memory allocated within this
- * this class.
- */
- ~IriMvbluefox3CameraDriverNode(void);
-
- /**
- * \brief main node thread
- *
- * This is the main thread node function. Code written here will be executed
- * in every node loop while the algorithm is on running state. Loop frequency
- * can be tuned by modifying loop rate attribute.
- */
- void mainNodeThread(void);
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ IriMvbluefox3CameraDriverNode(ros::NodeHandle &nh, dynamic_reconfigure::Server<Config> &dsrv);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~IriMvbluefox3CameraDriverNode(void);
+
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop rate attribute.
+ */
+ void mainNodeThread(void);
protected:
- /**
- * \brief dynamic reconfigure server callback
- *
- * This method is called whenever a new configuration is received through
- * the dynamic reconfigure. The derivated generic algorithm class must
- * implement it.
- *
- * \param config an object with new configuration from all algorithm
- * parameters defined in the config file.
- * \param level integer referring the level in which the configuration
- * has been changed.
- */
- void DynRecCallback(Config &config, uint32_t level=0);
-
- // [diagnostic functions]
-
- // [test functions]
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void DynRecCallback(Config &config, uint32_t level=0);
+
+ // [diagnostic functions]
+
+ // [test functions]
};
#endif
diff --git a/src/iri_mvbluefox3_camera_driver_alg.cpp b/src/iri_mvbluefox3_camera_driver_alg.cpp
index d3b6814..55d9e44 100644
--- a/src/iri_mvbluefox3_camera_driver_alg.cpp
+++ b/src/iri_mvbluefox3_camera_driver_alg.cpp
@@ -2,311 +2,311 @@
IriMvbluefox3CameraDriverAlgorithm::IriMvbluefox3CameraDriverAlgorithm(void)
{
- this->num_cams = 1;
- this->ini_user_params = false;
- this->ini_dynrec = true;
+ this->num_cams = 1;
+ this->ini_user_params = false;
+ this->ini_dynrec = true;
}
IriMvbluefox3CameraDriverAlgorithm::~IriMvbluefox3CameraDriverAlgorithm(void)
{
- // Close driver
- try
- {
- for (int ii = 0; ii < this->num_cams; ++ii)
- this->vcam_ptr[ii]->CloseDevice();
- std::cout << "[mvBlueFOX3]: Driver closed." << std::endl;
- }catch (CMvbluefox3::CmvBlueFOX3Exception& e)
- {
- std::cerr << e.what() << std::endl;
- }
+ // Close driver
+ try
+ {
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ this->vcam_ptr[ii]->CloseDevice();
+ std::cout << "[mvBlueFOX3]: Driver closed." << std::endl;
+ }catch (CMvbluefox3::CmvBlueFOX3Exception& e)
+ {
+ std::cerr << e.what() << std::endl;
+ }
- for (int ii = 0; ii < this->num_cams; ++ii)
- DelPtr(this->vcam_ptr[ii]);
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ DelPtr(this->vcam_ptr[ii]);
}
// IriMvbluefox3CameraDriverAlgorithm Public API
bool IriMvbluefox3CameraDriverAlgorithm::OpenDriver()
{
- try
- {
- for (int ii = 0; ii < this->num_cams; ++ii)
- {
- this->vcam_ptr[ii] = NULL;
+ try
+ {
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ {
+ this->vcam_ptr[ii] = NULL;
- if( !this->vserial[ii].empty() )
- {
- std::cout << "[mvBlueFOX3]: Trying to open device with serial: " << this->vserial[ii] << std::endl;
+ if( !this->vserial[ii].empty() )
+ {
+ std::cout << "[mvBlueFOX3]: Trying to open device with serial: " << this->vserial[ii] << std::endl;
- if (devMgr.getDeviceBySerial(this->vserial[ii]) == 0)
- {
- std::cerr << this->vcam_name[ii] << " not found! Unable to continue. " << std::endl;
- return true;
- }
- else
- {
- this->vcam_ptr[ii] = new CMvbluefox3::CMvbluefox3(devMgr.getDeviceBySerial(this->vserial[ii]),this->vparams_path[ii]);
- }
- }
- else
- {
- std::cerr << this->vcam_name[ii] << " not found because serial number is not specified!" << std::endl;
- return true;
- }
- }
- }catch (CMvbluefox3::CmvBlueFOX3Exception& e)
- {
- std::cerr << e.what() << std::endl;
- return false;
- }
- std::cout << "[mvBlueFOX3]: All cameras successfully opened." << std::endl;
+ if (devMgr.getDeviceBySerial(this->vserial[ii]) == 0)
+ {
+ std::cerr << this->vcam_name[ii] << " not found! Unable to continue. " << std::endl;
+ return true;
+ }
+ else
+ {
+ this->vcam_ptr[ii] = new CMvbluefox3::CMvbluefox3(devMgr.getDeviceBySerial(this->vserial[ii]),this->vparams_path[ii]);
+ }
+ }
+ else
+ {
+ std::cerr << this->vcam_name[ii] << " not found because serial number is not specified!" << std::endl;
+ return true;
+ }
+ }
+ }catch (CMvbluefox3::CmvBlueFOX3Exception& e)
+ {
+ std::cerr << e.what() << std::endl;
+ return false;
+ }
+ std::cout << "[mvBlueFOX3]: All cameras successfully opened." << std::endl;
- // Initialize parameters set from ROS node.
- if (this->ini_user_params)
- {
- for (int ii = 0; ii < this->num_cams; ++ii)
- SetConfig(ii);
- this->ini_user_params = false;
- }
- return true;
+ // Initialize parameters set from ROS node.
+ if (this->ini_user_params)
+ {
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ SetConfig(ii);
+ this->ini_user_params = false;
+ }
+ return true;
}
void IriMvbluefox3CameraDriverAlgorithm::ResizeObjs(void)
{
- this->vcam_ptr.resize(this->num_cams); // Camera pointers.
- this->vparams.resize(this->num_cams); // Camera parameters.
- this->vserial.resize(this->num_cams); // Serial numbers.
- this->vframe_id.resize(this->num_cams); // Frame ids.
- this->vcam_name.resize(this->num_cams); // Camera names.
- this->vparams_path.resize(this->num_cams); // Parameters' paths.
+ this->vcam_ptr.resize(this->num_cams); // Camera pointers.
+ this->vparams.resize(this->num_cams); // Camera parameters.
+ this->vserial.resize(this->num_cams); // Serial numbers.
+ this->vframe_id.resize(this->num_cams); // Frame ids.
+ this->vcam_name.resize(this->num_cams); // Camera names.
+ this->vparams_path.resize(this->num_cams); // Parameters' paths.
}
CMvbluefox3::CParams IriMvbluefox3CameraDriverAlgorithm::DynRecToParams(iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig &cfg)
{
- CMvbluefox3::CParams p;
+ CMvbluefox3::CParams p;
- p.pixel_format = PfToCodes(cfg.pixel_format);
- p.width = cfg.width;
- p.height = cfg.height;
- p.img_rot_angle = cfg.img_rot_angle;
- p.mirror = cfg.mirror;
- p.set_aoi = cfg.set_aoi;
- p.aoi_width = cfg.aoi_width;
- p.aoi_height = cfg.aoi_height;
- p.aoi_start_x = cfg.aoi_start_x;
- p.aoi_start_y = cfg.aoi_start_y;
- p.h_binning_enbl = cfg.h_binning_enbl;
- p.h_binning = cfg.h_binning;
- p.v_binning_enbl = cfg.v_binning_enbl;
- p.v_binning = cfg.v_binning;
- p.frame_rate = cfg.frame_rate;
- p.pixel_clock = cfg.pixel_clock;
- p.req_timeout = cfg.req_timeout;
- p.auto_ctrl_enbl = cfg.auto_ctrl_enbl;
- p.auto_ctrl_speed = cfg.auto_ctrl_speed;
- p.auto_expose = cfg.auto_expose;
- p.auto_expose_upperlimit = cfg.auto_expose_upperlimit;
- p.auto_expose_des_grey_val = cfg.auto_expose_des_grey_val;
- p.expose_us = cfg.expose_us;
- p.auto_gain = cfg.auto_gain;
- p.gain_db = cfg.gain_db;
- p.gamma = cfg.gamma;
- p.whiteblnce_auto_enbl = cfg.whiteblnce_auto_enbl;
- p.wb_r_gain = cfg.wb_r_gain;
- p.wb_b_gain = cfg.wb_b_gain;
- p.auto_blck_level = cfg.auto_blck_level;
- p.blck_level = cfg.blck_level;
- p.hdr_enbl = cfg.hdr_enbl;
- p.dark_cfilter = cfg.dark_cfilter;
- p.twist_cfilter = cfg.twist_cfilter;
+ p.pixel_format = PfToCodes(cfg.pixel_format);
+ p.width = cfg.width;
+ p.height = cfg.height;
+ p.img_rot_angle = cfg.img_rot_angle;
+ p.mirror = cfg.mirror;
+ p.set_aoi = cfg.set_aoi;
+ p.aoi_width = cfg.aoi_width;
+ p.aoi_height = cfg.aoi_height;
+ p.aoi_start_x = cfg.aoi_start_x;
+ p.aoi_start_y = cfg.aoi_start_y;
+ p.h_binning_enbl = cfg.h_binning_enbl;
+ p.h_binning = cfg.h_binning;
+ p.v_binning_enbl = cfg.v_binning_enbl;
+ p.v_binning = cfg.v_binning;
+ p.frame_rate = cfg.frame_rate;
+ p.pixel_clock = cfg.pixel_clock;
+ p.req_timeout = cfg.req_timeout;
+ p.auto_ctrl_enbl = cfg.auto_ctrl_enbl;
+ p.auto_ctrl_speed = cfg.auto_ctrl_speed;
+ p.auto_expose = cfg.auto_expose;
+ p.auto_expose_upperlimit = cfg.auto_expose_upperlimit;
+ p.auto_expose_des_grey_val = cfg.auto_expose_des_grey_val;
+ p.expose_us = cfg.expose_us;
+ p.auto_gain = cfg.auto_gain;
+ p.gain_db = cfg.gain_db;
+ p.gamma = cfg.gamma;
+ p.whiteblnce_auto_enbl = cfg.whiteblnce_auto_enbl;
+ p.wb_r_gain = cfg.wb_r_gain;
+ p.wb_b_gain = cfg.wb_b_gain;
+ p.auto_blck_level = cfg.auto_blck_level;
+ p.blck_level = cfg.blck_level;
+ p.hdr_enbl = cfg.hdr_enbl;
+ p.dark_cfilter = cfg.dark_cfilter;
+ p.twist_cfilter = cfg.twist_cfilter;
- return p;
+ return p;
}
iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig IriMvbluefox3CameraDriverAlgorithm::ParamsToDynRec(const int &cam_num, const CMvbluefox3::CParams &p)
{
- iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig cfg;
+ iri_mvbluefox3_camera_driver::IriMvbluefox3CameraDriverConfig cfg;
- cfg.apply_cfg = false;
- cfg.cam_name = this->vcam_name[cam_num];
- cfg.pixel_format = CodesToPf(p.pixel_format);
- cfg.width = p.width;
- cfg.height = p.height;
- cfg.img_rot_angle = p.img_rot_angle;
- cfg.mirror = p.mirror;
- cfg.set_aoi = p.set_aoi;
- cfg.aoi_width = p.aoi_width;
- cfg.aoi_height = p.aoi_height;
- cfg.aoi_start_x = p.aoi_start_x;
- cfg.aoi_start_y = p.aoi_start_y;
- cfg.h_binning_enbl = p.h_binning_enbl;
- cfg.h_binning = p.h_binning;
- cfg.v_binning_enbl = p.v_binning_enbl;
- cfg.v_binning = p.v_binning;
- cfg.frame_rate = p.frame_rate;
- cfg.pixel_clock = p.pixel_clock;
- cfg.req_timeout = p.req_timeout;
- cfg.auto_ctrl_enbl = p.auto_ctrl_enbl;
- cfg.auto_ctrl_speed = p.auto_ctrl_speed;
- cfg.auto_expose = p.auto_expose;
- cfg.auto_expose_upperlimit = p.auto_expose_upperlimit;
- cfg.auto_expose_des_grey_val = p.auto_expose_des_grey_val;
- cfg.expose_us = p.expose_us;
- cfg.auto_gain = p.auto_gain;
- cfg.gain_db = p.gain_db;
- cfg.gamma = p.gamma;
- cfg.whiteblnce_auto_enbl = p.whiteblnce_auto_enbl;
- cfg.wb_r_gain = p.wb_r_gain;
- cfg.wb_b_gain = p.wb_b_gain;
- cfg.auto_blck_level = p.auto_blck_level;
- cfg.blck_level = p.blck_level;
- cfg.hdr_enbl = p.hdr_enbl;
- cfg.dark_cfilter = p.dark_cfilter;
- cfg.twist_cfilter = p.twist_cfilter;
+ cfg.apply_cfg = false;
+ cfg.cam_name = this->vcam_name[cam_num];
+ cfg.pixel_format = CodesToPf(p.pixel_format);
+ cfg.width = p.width;
+ cfg.height = p.height;
+ cfg.img_rot_angle = p.img_rot_angle;
+ cfg.mirror = p.mirror;
+ cfg.set_aoi = p.set_aoi;
+ cfg.aoi_width = p.aoi_width;
+ cfg.aoi_height = p.aoi_height;
+ cfg.aoi_start_x = p.aoi_start_x;
+ cfg.aoi_start_y = p.aoi_start_y;
+ cfg.h_binning_enbl = p.h_binning_enbl;
+ cfg.h_binning = p.h_binning;
+ cfg.v_binning_enbl = p.v_binning_enbl;
+ cfg.v_binning = p.v_binning;
+ cfg.frame_rate = p.frame_rate;
+ cfg.pixel_clock = p.pixel_clock;
+ cfg.req_timeout = p.req_timeout;
+ cfg.auto_ctrl_enbl = p.auto_ctrl_enbl;
+ cfg.auto_ctrl_speed = p.auto_ctrl_speed;
+ cfg.auto_expose = p.auto_expose;
+ cfg.auto_expose_upperlimit = p.auto_expose_upperlimit;
+ cfg.auto_expose_des_grey_val = p.auto_expose_des_grey_val;
+ cfg.expose_us = p.expose_us;
+ cfg.auto_gain = p.auto_gain;
+ cfg.gain_db = p.gain_db;
+ cfg.gamma = p.gamma;
+ cfg.whiteblnce_auto_enbl = p.whiteblnce_auto_enbl;
+ cfg.wb_r_gain = p.wb_r_gain;
+ cfg.wb_b_gain = p.wb_b_gain;
+ cfg.auto_blck_level = p.auto_blck_level;
+ cfg.blck_level = p.blck_level;
+ cfg.hdr_enbl = p.hdr_enbl;
+ cfg.dark_cfilter = p.dark_cfilter;
+ cfg.twist_cfilter = p.twist_cfilter;
- return cfg;
+ return cfg;
}
CMvbluefox3::codings_t IriMvbluefox3CameraDriverAlgorithm::PfToCodes(int &pf)
{
- CMvbluefox3::codings_t ct;
- switch (pf)
- {
- case 0: ct = CMvbluefox3::raw; break;
- case 1: ct = CMvbluefox3::mono8; break;
- case 2: ct = CMvbluefox3::mono16; break;
- case 3: ct = CMvbluefox3::bgr8; break;
- case 4: ct = CMvbluefox3::bgr16; break;
- case 5: ct = CMvbluefox3::rgb8; break;
- case 6: ct = CMvbluefox3::yuv422; break;
- case 7: ct = CMvbluefox3::yuv444; break;
- }
- return ct;
+ CMvbluefox3::codings_t ct;
+ switch (pf)
+ {
+ case 0: ct = CMvbluefox3::raw; break;
+ case 1: ct = CMvbluefox3::mono8; break;
+ case 2: ct = CMvbluefox3::mono16; break;
+ case 3: ct = CMvbluefox3::bgr8; break;
+ case 4: ct = CMvbluefox3::bgr16; break;
+ case 5: ct = CMvbluefox3::rgb8; break;
+ case 6: ct = CMvbluefox3::yuv422; break;
+ case 7: ct = CMvbluefox3::yuv444; break;
+ }
+ return ct;
}
int IriMvbluefox3CameraDriverAlgorithm::CodesToPf(CMvbluefox3::codings_t ct)
{
- int pf;
- switch (ct)
- {
- case CMvbluefox3::raw: pf = 0; break;
- case CMvbluefox3::mono8: pf = 1; break;
- case CMvbluefox3::mono16: pf = 2; break;
- case CMvbluefox3::bgr8: pf = 3; break;
- case CMvbluefox3::bgr16: pf = 4; break;
- case CMvbluefox3::rgb8: pf = 5; break;
- case CMvbluefox3::yuv422: pf = 6; break;
- case CMvbluefox3::yuv444: pf = 7; break;
- }
- return pf;
+ int pf;
+ switch (ct)
+ {
+ case CMvbluefox3::raw: pf = 0; break;
+ case CMvbluefox3::mono8: pf = 1; break;
+ case CMvbluefox3::mono16: pf = 2; break;
+ case CMvbluefox3::bgr8: pf = 3; break;
+ case CMvbluefox3::bgr16: pf = 4; break;
+ case CMvbluefox3::rgb8: pf = 5; break;
+ case CMvbluefox3::yuv422: pf = 6; break;
+ case CMvbluefox3::yuv444: pf = 7; break;
+ }
+ return pf;
}
void IriMvbluefox3CameraDriverAlgorithm::SetConfig(const int &ncam)
{
- try
- {
- this->vcam_ptr[ncam]->SetConfig(this->vparams[ncam]);
- std::cout << "[mvBlueFOX3]: New configuration set for " << this->vcam_name[ncam] << "." << std::endl;
- }catch (CMvbluefox3::CmvBlueFOX3Exception& e)
- {
- std::cerr << e.what() << std::endl;
- }
+ try
+ {
+ this->vcam_ptr[ncam]->SetConfig(this->vparams[ncam]);
+ std::cout << "[mvBlueFOX3]: New configuration set for " << this->vcam_name[ncam] << "." << std::endl;
+ }catch (CMvbluefox3::CmvBlueFOX3Exception& e)
+ {
+ std::cerr << e.what() << std::endl;
+ }
}
void IriMvbluefox3CameraDriverAlgorithm::GetConfig(const int &ncam)
{
- this->vparams[ncam] = this->vcam_ptr[ncam]->GetConfig();
+ this->vparams[ncam] = this->vcam_ptr[ncam]->GetConfig();
}
void IriMvbluefox3CameraDriverAlgorithm::GetROSImage(const int &ncam, sensor_msgs::Image &img_msg)
{
- ros::Time start_time = ros::Time::now();
+ ros::Time start_time = ros::Time::now();
- char *image = NULL;
- this->vcam_ptr[ncam]->GetImage(&image);
- GetConfig(ncam);
+ char *image = NULL;
+ this->vcam_ptr[ncam]->GetImage(&image);
+ GetConfig(ncam);
- std::string encoding;
- encoding = CMvbluefox3::codings_str[this->vparams[ncam].pixel_format];
+ std::string encoding;
+ encoding = CMvbluefox3::codings_str[this->vparams[ncam].pixel_format];
- int bp = this->vcam_ptr[ncam]->GetBytesPixel();
- int depth = this->vcam_ptr[ncam]->GetDepth();
+ int bp = this->vcam_ptr[ncam]->GetBytesPixel();
+ int depth = this->vcam_ptr[ncam]->GetDepth();
- bool ROSvalid = true;
- switch (this->vparams[ncam].pixel_format)
- {
- case CMvbluefox3::raw:
+ bool ROSvalid = true;
+ switch (this->vparams[ncam].pixel_format)
+ {
+ case CMvbluefox3::raw:
- if (depth/8 == 1)
- {
- encoding = "8UC1";
- switch (bp)
- {
- case 1: encoding = "8UC1"; break;
- case 2: encoding = "8UC2"; break;
- case 3: encoding = "8UC3"; break;
- case 4: encoding = "8UC4"; break;
- default: ROS_WARN("Invalid depth. Depth not implemented here."); break;
- }
- }
- else if (depth/8 == 2)
- {
- encoding = "16UC1";
- switch (bp)
- {
- case 1: encoding = "16UC1"; break;
- case 2: encoding = "16UC2"; break;
- case 3: encoding = "16UC3"; break;
- case 4: encoding = "16UC4"; break;
- default: ROS_WARN("Invalid depth. Depth not implemented here."); break;
- }
- }
- else
- ROS_WARN("Invalid depth. Depth not implemented here.");
- break;
- case CMvbluefox3::mono8: break;
- case CMvbluefox3::mono10: ROSvalid = false; break;
- case CMvbluefox3::mono12: ROSvalid = false; break;
- case CMvbluefox3::mono12v1: ROSvalid = false; break;
- case CMvbluefox3::mono12v2: ROSvalid = false; break;
- case CMvbluefox3::mono14: ROSvalid = false; break;
- case CMvbluefox3::mono16: encoding = "16UC1"; break;
- case CMvbluefox3::rgb8: break;
- case CMvbluefox3::bgr8: break;
- case CMvbluefox3::bgr8p: ROSvalid = false; break;
- case CMvbluefox3::bgra8p: ROSvalid = false; break;
- case CMvbluefox3::bgr10: ROSvalid = false; break;
- case CMvbluefox3::bgr12: ROSvalid = false; break;
- case CMvbluefox3::bgr14: ROSvalid = false; break;
- case CMvbluefox3::bgr16: encoding = "16UC3"; break;
- case CMvbluefox3::bgra8: ROSvalid = false; break;
- case CMvbluefox3::yuv411uyyvyy: ROSvalid = false; break;
- case CMvbluefox3::yuv422: break;
- case CMvbluefox3::yuv422p: ROSvalid = false; break;
- case CMvbluefox3::yuv42210: ROSvalid = false; break;
- case CMvbluefox3::yuv422uyvy: ROSvalid = false; break;
- case CMvbluefox3::yuv422uyvy10: ROSvalid = false; break;
- case CMvbluefox3::yuv444: ROSvalid = false; break;
- case CMvbluefox3::yuv44410p: ROSvalid = false; break;
- case CMvbluefox3::yuv444uyv: ROSvalid = false; break;
- case CMvbluefox3::yuv444uyv10: ROSvalid = false; break;
- }
+ if (depth/8 == 1)
+ {
+ encoding = "8UC1";
+ switch (bp)
+ {
+ case 1: encoding = "8UC1"; break;
+ case 2: encoding = "8UC2"; break;
+ case 3: encoding = "8UC3"; break;
+ case 4: encoding = "8UC4"; break;
+ default: ROS_WARN("Invalid depth. Depth not implemented here."); break;
+ }
+ }
+ else if (depth/8 == 2)
+ {
+ encoding = "16UC1";
+ switch (bp)
+ {
+ case 1: encoding = "16UC1"; break;
+ case 2: encoding = "16UC2"; break;
+ case 3: encoding = "16UC3"; break;
+ case 4: encoding = "16UC4"; break;
+ default: ROS_WARN("Invalid depth. Depth not implemented here."); break;
+ }
+ }
+ else
+ ROS_WARN("Invalid depth. Depth not implemented here.");
+ break;
+ case CMvbluefox3::mono8: break;
+ case CMvbluefox3::mono10: ROSvalid = false; break;
+ case CMvbluefox3::mono12: ROSvalid = false; break;
+ case CMvbluefox3::mono12v1: ROSvalid = false; break;
+ case CMvbluefox3::mono12v2: ROSvalid = false; break;
+ case CMvbluefox3::mono14: ROSvalid = false; break;
+ case CMvbluefox3::mono16: encoding = "16UC1"; break;
+ case CMvbluefox3::rgb8: break;
+ case CMvbluefox3::bgr8: break;
+ case CMvbluefox3::bgr8p: ROSvalid = false; break;
+ case CMvbluefox3::bgra8p: ROSvalid = false; break;
+ case CMvbluefox3::bgr10: ROSvalid = false; break;
+ case CMvbluefox3::bgr12: ROSvalid = false; break;
+ case CMvbluefox3::bgr14: ROSvalid = false; break;
+ case CMvbluefox3::bgr16: encoding = "16UC3"; break;
+ case CMvbluefox3::bgra8: ROSvalid = false; break;
+ case CMvbluefox3::yuv411uyyvyy: ROSvalid = false; break;
+ case CMvbluefox3::yuv422: break;
+ case CMvbluefox3::yuv422p: ROSvalid = false; break;
+ case CMvbluefox3::yuv42210: ROSvalid = false; break;
+ case CMvbluefox3::yuv422uyvy: ROSvalid = false; break;
+ case CMvbluefox3::yuv422uyvy10: ROSvalid = false; break;
+ case CMvbluefox3::yuv444: ROSvalid = false; break;
+ case CMvbluefox3::yuv44410p: ROSvalid = false; break;
+ case CMvbluefox3::yuv444uyv: ROSvalid = false; break;
+ case CMvbluefox3::yuv444uyv10: ROSvalid = false; break;
+ }
- if (!ROSvalid)
- ROS_WARN("mvBlueFOX3 pixel format %s no valid within ROS messages.",encoding.c_str());
+ if (!ROSvalid)
+ ROS_WARN("mvBlueFOX3 pixel format %s no valid within ROS messages.",encoding.c_str());
- sensor_msgs::fillImage(img_msg,
- encoding,
- this->vparams[ncam].height,
- this->vparams[ncam].width,
- this->vcam_ptr[ncam]->GetImgLinePitch(),
- image);
+ sensor_msgs::fillImage(img_msg,
+ encoding,
+ this->vparams[ncam].height,
+ this->vparams[ncam].width,
+ this->vcam_ptr[ncam]->GetImgLinePitch(),
+ image);
- double req_dur_sec = (double)(this->vcam_ptr[ncam]->ReqExposeUs())*1e-6;
- img_msg.header.stamp = start_time + ros::Duration(req_dur_sec/2.0);
- img_msg.header.frame_id = this->vframe_id[ncam];
+ double req_dur_sec = (double)(this->vcam_ptr[ncam]->ReqExposeUs())*1e-6;
+ img_msg.header.stamp = start_time + ros::Duration(req_dur_sec/2.0);
+ img_msg.header.frame_id = this->vframe_id[ncam];
- delete [] image;
+ delete [] image;
}
diff --git a/src/iri_mvbluefox3_camera_driver_node.cpp b/src/iri_mvbluefox3_camera_driver_node.cpp
index da0a60e..d8e1c38 100644
--- a/src/iri_mvbluefox3_camera_driver_node.cpp
+++ b/src/iri_mvbluefox3_camera_driver_node.cpp
@@ -4,199 +4,199 @@ IriMvbluefox3CameraDriverNode::IriMvbluefox3CameraDriverNode(ros::NodeHandle &nh
initialized_(false),
it(nh)
{
- // Dynamic Reconfigure
- dynamic_reconfigure::Server<Config>::CallbackType dsrv_cb;
- dsrv_cb = boost::bind(&IriMvbluefox3CameraDriverNode::DynRecCallback, this, _1, _2);
- dsrv.setCallback(dsrv_cb);
-
- //init class attributes if necessary
-
- this->alg_.lock();
-
- // Get launch parameters
- //double d_example;
- //nh.param<double>("d_example",d_example,0.5);
- nh.param<int>("num_cams", this->alg_.num_cams, 1);
-
- // Resize all objects
- this->alg_.ResizeObjs();
- this->camera_manager.resize(this->alg_.num_cams);
- this->image_publisher_.resize(this->alg_.num_cams);
- this->image_msg_.resize(this->alg_.num_cams);
-
- // [init publishers]
- // this->camera_driver_info_publisher_ = nh.advertise<sensor_msgs::CameraInfo>("camera_driver_info", 1);
- // this->image_raw_publisher_ = this->it.advertiseCamera("image_raw/image_raw", 1);
- // // uncomment the following lines to load the calibration file for the camera_driver
- // // Change <cal_file_param> for the correct parameter name holding the configuration filename
- // std::string image_raw_cal_file;
- // nh.param<std::string>("camera_driver_calibration_yaml",image_raw_cal_file,"");
- // if(this->image_raw_camera_manager.validateURL(image_raw_cal_file))
- // {
- // if(!this->image_raw_camera_manager.loadCameraInfo(image_raw_cal_file))
- // ROS_ERROR("Invalid calibration file");
- // }
- // else
- // ROS_ERROR("Invalid calibration file");
- //
-
- for (int ii = 0; ii < this->alg_.num_cams; ++ii)
- {
- // Generate camera_driver name
- std::stringstream cn;
- cn << "c" << ii+1;
-
- // get camera_driver name
- std::string cam_name = cn.str() + "/name";
- nh.param<std::string>(cam_name, this->alg_.vcam_name[ii], "c1");
-
- // generate camera_driver manager
- std::string cam_mgr_name = "~" + this->alg_.vcam_name[ii] + std::string("/image_raw");
- this->camera_manager[ii] = new camera_info_manager::CameraInfoManager(cam_mgr_name);
-
- // get topic name adn advertise publisher
- std::string img_name = this->alg_.vcam_name[ii] + "/image_raw";
- this->image_publisher_[ii] = this->it.advertiseCamera(img_name, 1);
-
- // Constant parameters
- std::string frame_id, tf_prefix, image_cal_file;
- std::string frame_id_name = cn.str() + "/frame_id";
- std::string tf_prefix_name = cn.str() + "/tf_prefix";
- std::string serial_name = cn.str() + "/serial";
- std::string params_path_name = cn.str() + "/params_path";
- std::string image_cal_file_name = cn.str() + "/calib_file";
- nh.param<std::string>(frame_id_name, frame_id, "");
- nh.param<std::string>(tf_prefix_name, tf_prefix, "");
- this->alg_.vframe_id[ii] = tf_prefix + "/" + frame_id;
- nh.param<std::string>(serial_name, this->alg_.vserial[ii], "");
- nh.param<std::string>(params_path_name, this->alg_.vparams_path[ii], "");
- nh.param<std::string>(image_cal_file_name,image_cal_file,"");
-
- // Get calibration file
- std::stringstream err_msg;
- err_msg << "Invalid calibration file for " << this->alg_.vcam_name[ii];
- if(this->camera_manager[ii]->validateURL(image_cal_file))
- {
- if(!this->camera_manager[ii]->loadCameraInfo(image_cal_file))
- ROS_WARN("%s", err_msg.str().c_str());
- }
- else
- ROS_WARN("%s", err_msg.str().c_str());
-
- // Get and Set camera_driver parameters.
- // this function is not used here because loading parameters
- // from an XML file is a preferred method
- // (e.g. use wxPropView to store it and specify its path
- // in the launch file).
- // However in case of preferring to configure only main settings
- // with a simpler file, uncomment the following function (see 1) and load a YAML file in the launch file (see step 2).
- // An example of YAML file is added in the /params folder but hidden
- // (.F0300141_params.yaml)
-
- // 1) UNCOMMENT:
- // SetParams(nh, this->alg_.vcam_name[ii]);
- // 2) Add in the node section of launch file (example): <rosparam ns="c1" file="$(find iri_mvbluefox3_camera_driver)/params/F0300141_params.yaml" command="load"/>
- }
-
- this->alg_.unlock();
-
- // [init subscribers]
-
- // [init services]
-
- // [init clients]
-
- // [init action servers]
-
- // [init action clients]
+ // Dynamic Reconfigure
+ dynamic_reconfigure::Server<Config>::CallbackType dsrv_cb;
+ dsrv_cb = boost::bind(&IriMvbluefox3CameraDriverNode::DynRecCallback, this, _1, _2);
+ dsrv.setCallback(dsrv_cb);
+
+ //init class attributes if necessary
+
+ this->alg_.lock();
+
+ // Get launch parameters
+ //double d_example;
+ //nh.param<double>("d_example",d_example,0.5);
+ nh.param<int>("num_cams", this->alg_.num_cams, 1);
+
+ // Resize all objects
+ this->alg_.ResizeObjs();
+ this->camera_manager.resize(this->alg_.num_cams);
+ this->image_publisher_.resize(this->alg_.num_cams);
+ this->image_msg_.resize(this->alg_.num_cams);
+
+ // [init publishers]
+ // this->camera_driver_info_publisher_ = nh.advertise<sensor_msgs::CameraInfo>("camera_driver_info", 1);
+ // this->image_raw_publisher_ = this->it.advertiseCamera("image_raw/image_raw", 1);
+ // // uncomment the following lines to load the calibration file for the camera_driver
+ // // Change <cal_file_param> for the correct parameter name holding the configuration filename
+ // std::string image_raw_cal_file;
+ // nh.param<std::string>("camera_driver_calibration_yaml",image_raw_cal_file,"");
+ // if(this->image_raw_camera_manager.validateURL(image_raw_cal_file))
+ // {
+ // if(!this->image_raw_camera_manager.loadCameraInfo(image_raw_cal_file))
+ // ROS_ERROR("Invalid calibration file");
+ // }
+ // else
+ // ROS_ERROR("Invalid calibration file");
+ //
+
+ for (int ii = 0; ii < this->alg_.num_cams; ++ii)
+ {
+ // Generate camera_driver name
+ std::stringstream cn;
+ cn << "c" << ii+1;
+
+ // get camera_driver name
+ std::string cam_name = cn.str() + "/name";
+ nh.param<std::string>(cam_name, this->alg_.vcam_name[ii], "c1");
+
+ // generate camera_driver manager
+ std::string cam_mgr_name = "~" + this->alg_.vcam_name[ii] + std::string("/image_raw");
+ this->camera_manager[ii] = new camera_info_manager::CameraInfoManager(cam_mgr_name);
+
+ // get topic name adn advertise publisher
+ std::string img_name = this->alg_.vcam_name[ii] + "/image_raw";
+ this->image_publisher_[ii] = this->it.advertiseCamera(img_name, 1);
+
+ // Constant parameters
+ std::string frame_id, tf_prefix, image_cal_file;
+ std::string frame_id_name = cn.str() + "/frame_id";
+ std::string tf_prefix_name = cn.str() + "/tf_prefix";
+ std::string serial_name = cn.str() + "/serial";
+ std::string params_path_name = cn.str() + "/params_path";
+ std::string image_cal_file_name = cn.str() + "/calib_file";
+ nh.param<std::string>(frame_id_name, frame_id, "");
+ nh.param<std::string>(tf_prefix_name, tf_prefix, "");
+ this->alg_.vframe_id[ii] = tf_prefix + "/" + frame_id;
+ nh.param<std::string>(serial_name, this->alg_.vserial[ii], "");
+ nh.param<std::string>(params_path_name, this->alg_.vparams_path[ii], "");
+ nh.param<std::string>(image_cal_file_name,image_cal_file,"");
+
+ // Get calibration file
+ std::stringstream err_msg;
+ err_msg << "Invalid calibration file for " << this->alg_.vcam_name[ii];
+ if(this->camera_manager[ii]->validateURL(image_cal_file))
+ {
+ if(!this->camera_manager[ii]->loadCameraInfo(image_cal_file))
+ ROS_WARN("%s", err_msg.str().c_str());
+ }
+ else
+ ROS_WARN("%s", err_msg.str().c_str());
+
+ // Get and Set camera_driver parameters.
+ // this function is not used here because loading parameters
+ // from an XML file is a preferred method
+ // (e.g. use wxPropView to store it and specify its path
+ // in the launch file).
+ // However in case of preferring to configure only main settings
+ // with a simpler file, uncomment the following function (see 1) and load a YAML file in the launch file (see step 2).
+ // An example of YAML file is added in the /params folder but hidden
+ // (.F0300141_params.yaml)
+
+ // 1) UNCOMMENT:
+ // SetParams(nh, this->alg_.vcam_name[ii]);
+ // 2) Add in the node section of launch file (example): <rosparam ns="c1" file="$(find iri_mvbluefox3_camera_driver)/params/F0300141_params.yaml" command="load"/>
+ }
+
+ this->alg_.unlock();
+
+ // [init subscribers]
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
}
IriMvbluefox3CameraDriverNode::~IriMvbluefox3CameraDriverNode(void)
{
- // [free dynamic memory]
- for (int ii = 0; ii < this->alg_.num_cams; ++ii)
- this->alg_.DelPtr(this->camera_manager[ii]);
+ // [free dynamic memory]
+ for (int ii = 0; ii < this->alg_.num_cams; ++ii)
+ this->alg_.DelPtr(this->camera_manager[ii]);
}
void IriMvbluefox3CameraDriverNode::mainNodeThread(void)
{
- // [fill msg structures]
+ // [fill msg structures]
- // [fill srv structure and make request to the server]
+ // [fill srv structure and make request to the server]
- // [fill action structure and make request to the action server]
+ // [fill action structure and make request to the action server]
- // [publish messages]
+ // [publish messages]
- if (!this->initialized_)
- {
- if(this->alg_.OpenDriver())
- this->initialized_ = true;
- }
+ if (!this->initialized_)
+ {
+ if(this->alg_.OpenDriver())
+ this->initialized_ = true;
+ }
- // Image
- for (int ii = 0; ii < this->alg_.num_cams; ++ii)
- {
- this->alg_.lock();
- this->alg_.GetROSImage(ii,this->image_msg_[ii]);
- this->alg_.unlock();
+ // Image
+ for (int ii = 0; ii < this->alg_.num_cams; ++ii)
+ {
+ this->alg_.lock();
+ this->alg_.GetROSImage(ii,this->image_msg_[ii]);
+ this->alg_.unlock();
- // CameraDriver info
- sensor_msgs::CameraInfo camera_driver_info = this->camera_manager[ii]->getCameraInfo();
- camera_driver_info.header = this->image_msg_[ii].header;
- this->image_publisher_[ii].publish(this->image_msg_[ii],camera_driver_info);
- }
+ // CameraDriver info
+ sensor_msgs::CameraInfo camera_driver_info = this->camera_manager[ii]->getCameraInfo();
+ camera_driver_info.header = this->image_msg_[ii].header;
+ this->image_publisher_[ii].publish(this->image_msg_[ii],camera_driver_info);
+ }
}
// Optional function. See Class constructor.
void IriMvbluefox3CameraDriverNode::SetParams(const ros::NodeHandle &nh, const std::string &cam_name)
{
- std::string node_str;
-
- for (int ii = 0; ii < this->alg_.num_cams; ++ii)
- {
- node_str = ros::this_node::getName() + "/" + this->alg_.vcam_name[ii] + "/";
-
- int pf;
- nh.getParam(node_str + "pixel_format", pf);
- this->alg_.vparams[ii].pixel_format = this->alg_.PfToCodes(pf);
- nh.getParam(node_str + "width", this->alg_.vparams[ii].width);
- nh.getParam(node_str + "height", this->alg_.vparams[ii].height);
- nh.getParam(node_str + "img_rot_angle", this->alg_.vparams[ii].img_rot_angle);
- nh.getParam(node_str + "mirror", this->alg_.vparams[ii].mirror);
- nh.getParam(node_str + "set_aoi", this->alg_.vparams[ii].set_aoi);
- nh.getParam(node_str + "aoi_width", this->alg_.vparams[ii].aoi_width);
- nh.getParam(node_str + "aoi_height", this->alg_.vparams[ii].aoi_height);
- nh.getParam(node_str + "aoi_start_x", this->alg_.vparams[ii].aoi_start_x);
- nh.getParam(node_str + "aoi_start_y", this->alg_.vparams[ii].aoi_start_y);
- nh.getParam(node_str + "h_binning_enbl", this->alg_.vparams[ii].h_binning_enbl);
- nh.getParam(node_str + "h_binning", this->alg_.vparams[ii].h_binning);
- nh.getParam(node_str + "v_binning_enbl", this->alg_.vparams[ii].v_binning_enbl);
- nh.getParam(node_str + "v_binning", this->alg_.vparams[ii].v_binning);
- nh.getParam(node_str + "frame_rate", this->alg_.vparams[ii].frame_rate);
- nh.getParam(node_str + "pixel_clock", this->alg_.vparams[ii].pixel_clock);
- nh.getParam(node_str + "req_timeout", this->alg_.vparams[ii].req_timeout);
- nh.getParam(node_str + "auto_ctrl_enbl", this->alg_.vparams[ii].auto_ctrl_enbl);
- nh.getParam(node_str + "auto_ctrl_speed", this->alg_.vparams[ii].auto_ctrl_speed);
- nh.getParam(node_str + "auto_expose", this->alg_.vparams[ii].auto_expose);
- nh.getParam(node_str + "auto_expose_upperlimit", this->alg_.vparams[ii].auto_expose_upperlimit);
- nh.getParam(node_str + "auto_expose_des_grey_val", this->alg_.vparams[ii].auto_expose_des_grey_val);
- nh.getParam(node_str + "expose_us", this->alg_.vparams[ii].expose_us);
- nh.getParam(node_str + "auto_gain", this->alg_.vparams[ii].auto_gain);
- nh.getParam(node_str + "gain_db", this->alg_.vparams[ii].gain_db);
- nh.getParam(node_str + "gamma", this->alg_.vparams[ii].gamma);
- nh.getParam(node_str + "whiteblnce_auto_enbl", this->alg_.vparams[ii].whiteblnce_auto_enbl);
- nh.getParam(node_str + "wb_r_gain", this->alg_.vparams[ii].wb_r_gain);
- nh.getParam(node_str + "wb_b_gain", this->alg_.vparams[ii].wb_b_gain);
- nh.getParam(node_str + "blck_level", this->alg_.vparams[ii].blck_level);
- nh.getParam(node_str + "auto_blck_level", this->alg_.vparams[ii].auto_blck_level);
- nh.getParam(node_str + "hdr_enbl", this->alg_.vparams[ii].hdr_enbl);
- nh.getParam(node_str + "dark_cfilter", this->alg_.vparams[ii].dark_cfilter);
- nh.getParam(node_str + "twist_cfilter", this->alg_.vparams[ii].twist_cfilter);
- }
-
- this->alg_.ini_user_params = true;
+ std::string node_str;
+
+ for (int ii = 0; ii < this->alg_.num_cams; ++ii)
+ {
+ node_str = ros::this_node::getName() + "/" + this->alg_.vcam_name[ii] + "/";
+
+ int pf;
+ nh.getParam(node_str + "pixel_format", pf);
+ this->alg_.vparams[ii].pixel_format = this->alg_.PfToCodes(pf);
+ nh.getParam(node_str + "width", this->alg_.vparams[ii].width);
+ nh.getParam(node_str + "height", this->alg_.vparams[ii].height);
+ nh.getParam(node_str + "img_rot_angle", this->alg_.vparams[ii].img_rot_angle);
+ nh.getParam(node_str + "mirror", this->alg_.vparams[ii].mirror);
+ nh.getParam(node_str + "set_aoi", this->alg_.vparams[ii].set_aoi);
+ nh.getParam(node_str + "aoi_width", this->alg_.vparams[ii].aoi_width);
+ nh.getParam(node_str + "aoi_height", this->alg_.vparams[ii].aoi_height);
+ nh.getParam(node_str + "aoi_start_x", this->alg_.vparams[ii].aoi_start_x);
+ nh.getParam(node_str + "aoi_start_y", this->alg_.vparams[ii].aoi_start_y);
+ nh.getParam(node_str + "h_binning_enbl", this->alg_.vparams[ii].h_binning_enbl);
+ nh.getParam(node_str + "h_binning", this->alg_.vparams[ii].h_binning);
+ nh.getParam(node_str + "v_binning_enbl", this->alg_.vparams[ii].v_binning_enbl);
+ nh.getParam(node_str + "v_binning", this->alg_.vparams[ii].v_binning);
+ nh.getParam(node_str + "frame_rate", this->alg_.vparams[ii].frame_rate);
+ nh.getParam(node_str + "pixel_clock", this->alg_.vparams[ii].pixel_clock);
+ nh.getParam(node_str + "req_timeout", this->alg_.vparams[ii].req_timeout);
+ nh.getParam(node_str + "auto_ctrl_enbl", this->alg_.vparams[ii].auto_ctrl_enbl);
+ nh.getParam(node_str + "auto_ctrl_speed", this->alg_.vparams[ii].auto_ctrl_speed);
+ nh.getParam(node_str + "auto_expose", this->alg_.vparams[ii].auto_expose);
+ nh.getParam(node_str + "auto_expose_upperlimit", this->alg_.vparams[ii].auto_expose_upperlimit);
+ nh.getParam(node_str + "auto_expose_des_grey_val", this->alg_.vparams[ii].auto_expose_des_grey_val);
+ nh.getParam(node_str + "expose_us", this->alg_.vparams[ii].expose_us);
+ nh.getParam(node_str + "auto_gain", this->alg_.vparams[ii].auto_gain);
+ nh.getParam(node_str + "gain_db", this->alg_.vparams[ii].gain_db);
+ nh.getParam(node_str + "gamma", this->alg_.vparams[ii].gamma);
+ nh.getParam(node_str + "whiteblnce_auto_enbl", this->alg_.vparams[ii].whiteblnce_auto_enbl);
+ nh.getParam(node_str + "wb_r_gain", this->alg_.vparams[ii].wb_r_gain);
+ nh.getParam(node_str + "wb_b_gain", this->alg_.vparams[ii].wb_b_gain);
+ nh.getParam(node_str + "blck_level", this->alg_.vparams[ii].blck_level);
+ nh.getParam(node_str + "auto_blck_level", this->alg_.vparams[ii].auto_blck_level);
+ nh.getParam(node_str + "hdr_enbl", this->alg_.vparams[ii].hdr_enbl);
+ nh.getParam(node_str + "dark_cfilter", this->alg_.vparams[ii].dark_cfilter);
+ nh.getParam(node_str + "twist_cfilter", this->alg_.vparams[ii].twist_cfilter);
+ }
+
+ this->alg_.ini_user_params = true;
}
/* [subscriber callbacks] */
@@ -209,61 +209,61 @@ void IriMvbluefox3CameraDriverNode::SetParams(const ros::NodeHandle &nh, const s
void IriMvbluefox3CameraDriverNode::DynRecCallback(Config &config, uint32_t level)
{
- this->alg_.lock();
-
- if (this->initialized_)
- {
-
- this->config_=config;
-
-
- // Initialize dynamic reconfigure with first camera_driver
- if (this->alg_.ini_dynrec && this->alg_.num_cams > 0)
- {
- this->alg_.GetConfig(0);
- config = this->alg_.ParamsToDynRec(0,this->alg_.vparams[0]);
- this->alg_.ini_dynrec = false;
- }
-
- // Set new config is requested
- if (config.apply_cfg)
- {
- for (int ii = 0; ii < this->alg_.num_cams; ++ii)
- {
- if (this->alg_.vcam_name[ii].compare(config.cam_name) == 0)
- {
- // Fill parameters
- this->alg_.vparams[ii] = this->alg_.DynRecToParams(config);
- this->alg_.SetConfig(ii);
- this->alg_.GetConfig(ii);
- config = this->alg_.ParamsToDynRec(ii,this->alg_.vparams[ii]);
- }
- }
- config.apply_cfg = false;
- }
-
- }
-
- this->alg_.unlock();
+ this->alg_.lock();
+
+ if (this->initialized_)
+ {
+
+ this->config_=config;
+
+
+ // Initialize dynamic reconfigure with first camera_driver
+ if (this->alg_.ini_dynrec && this->alg_.num_cams > 0)
+ {
+ this->alg_.GetConfig(0);
+ config = this->alg_.ParamsToDynRec(0,this->alg_.vparams[0]);
+ this->alg_.ini_dynrec = false;
+ }
+
+ // Set new config is requested
+ if (config.apply_cfg)
+ {
+ for (int ii = 0; ii < this->alg_.num_cams; ++ii)
+ {
+ if (this->alg_.vcam_name[ii].compare(config.cam_name) == 0)
+ {
+ // Fill parameters
+ this->alg_.vparams[ii] = this->alg_.DynRecToParams(config);
+ this->alg_.SetConfig(ii);
+ this->alg_.GetConfig(ii);
+ config = this->alg_.ParamsToDynRec(ii,this->alg_.vparams[ii]);
+ }
+ }
+ config.apply_cfg = false;
+ }
+
+ }
+
+ this->alg_.unlock();
}
/* main function */
int main(int argc,char *argv[])
{
- ros::init(argc, argv, "iri_mvbluefox3_camera_driver");
- ros::NodeHandle nh(ros::this_node::getName());
- ros::Rate loop_rate(50);
+ ros::init(argc, argv, "iri_mvbluefox3_camera_driver");
+ ros::NodeHandle nh(ros::this_node::getName());
+ ros::Rate loop_rate(50);
- dynamic_reconfigure::Server<Config> dsrv;
+ dynamic_reconfigure::Server<Config> dsrv;
- IriMvbluefox3CameraDriverNode node(nh, dsrv);
+ IriMvbluefox3CameraDriverNode node(nh, dsrv);
- while (ros::ok())
- {
- node.mainNodeThread();
- ros::spinOnce();
- loop_rate.sleep();
- }
+ while (ros::ok())
+ {
+ node.mainNodeThread();
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
- return 0;
+ return 0;
}
--
GitLab