diff --git a/include/mvbluefox3_camera_driver.h b/include/mvbluefox3_camera_driver.h
index d1f664ef0070cc4e7e3f0e4f2bceb9a0b6d5364c..1389be07c15f553461c2d6b6a64a7576494217d7 100644
--- a/include/mvbluefox3_camera_driver.h
+++ b/include/mvbluefox3_camera_driver.h
@@ -63,34 +63,19 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
std::vector<CMvbluefox3::CParams> vparams; // 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 vparams_ object (to be filled externally).
*/
- void SetConfig(void);
+ void SetConfig(const int &ncam);
/**
* \brief Get Configuration
*
* Get Configuration.
*/
- void GetConfig(void);
+ void GetConfig(const int &ncam);
/**
* \brief Dynamic reconfigure to camera parameters
@@ -205,8 +190,31 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
* Get image.
*/
// void GetImage(char *image);
- void GetROSImage(sensor_msgs::Image &img_msg);
+ void GetROSImage(const int &ncam, sensor_msgs::Image &img_msg);
+
+ /**
+ * \brief Set objects in driver class
+ *
+ * Set objects in driver class.
+ *
+ */
+ void SetObjs(void);
+ /**
+ * \brief Delete pointer
+ *
+ * Function to delete pointers.
+ */
+ template <typename Ptr_t>
+ void DelPtr(Ptr_t &ptr)
+ {
+ if (ptr != NULL)
+ {
+ ptr = NULL;
+ delete [] ptr;
+ }
+ }
+
/**
* \brief Destructor
*
diff --git a/include/mvbluefox3_camera_driver_node.h b/include/mvbluefox3_camera_driver_node.h
index 374cd7f13d8a0e78f9054d7a5a8ec056bcba0c12..7019523cdc3c31690c312c1034e5008cb5f9b805 100644
--- a/include/mvbluefox3_camera_driver_node.h
+++ b/include/mvbluefox3_camera_driver_node.h
@@ -28,6 +28,8 @@
#include <iri_base_driver/iri_base_driver_node.h>
#include "mvbluefox3_camera_driver.h"
+#include <sstream>
+
// [publisher subscriber headers]
#include <camera_info_manager/camera_info_manager.h>
#include <image_transport/image_transport.h>
@@ -66,11 +68,10 @@ class Mvbluefox3CameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Mvb
//cv_bridge::CvImagePtr cv_image_;
image_transport::ImageTransport it;
- std::vector<camera_info_manager::CameraInfoManager> *camera_manager;
+ std::vector<camera_info_manager::CameraInfoManager*> camera_manager;
std::vector<image_transport::CameraPublisher> image_publisher_;
std::vector<sensor_msgs::Image> image_msg_;
-
// [subscriber attributes]
// [service attributes]
@@ -118,6 +119,7 @@ class Mvbluefox3CameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Mvb
*/
~Mvbluefox3CameraDriverNode(void);
+
protected:
/**
* \brief main node thread
diff --git a/launch/iri_mvbluefox3_camera.launch b/launch/iri_mvbluefox3_camera.launch
index ecea73c46c314be3245f58f90563b98a8b3f868d..2ebf2332fa6473c789567bd1e760d26ebdd83d09 100644
--- a/launch/iri_mvbluefox3_camera.launch
+++ b/launch/iri_mvbluefox3_camera.launch
@@ -5,11 +5,17 @@
name="iri_mvbluefox3_camera"
type="iri_mvbluefox3_camera"
output="screen">
- <param name="serial" value="F0300141"/>
- <param name="frame_id" value="mvBlueFOX3_camera"/>
- <param name="tf_preix" value=""/>
- <param name="calibration_file" value="$(find iri_mvbluefox3_camera)/params/F0300141_calib.yaml"/>
+ <param name="num_cams" value="2"/>
+ <param name="cam1/serial" value="F0300141"/>
+ <param name="cam1/frame_id" value="mvBlueFOX3_camera"/>
+ <param name="cam1/tf_preix" value=""/>
+ <param name="cam1/calibration_file" value="$(find iri_mvbluefox3_camera)/params/F0300141_calib.yaml"/>
<rosparam command="load" file="$(find iri_mvbluefox3_camera)/params/F0300141_params.yaml" />
+
+ <param name="cam2/serial" value="F0300123"/>
+ <param name="cam2/frame_id" value="mvBlueFOX3_camera"/>
+ <param name="cam2/tf_preix" value=""/>
+ <param name="cam2/calibration_file" value="$(find iri_mvbluefox3_camera)/params/F0300141_calib.yaml"/>
</node>
<!-- <node pkg="iri_mvbluefox3_camera"
diff --git a/src/mvbluefox3_camera_driver.cpp b/src/mvbluefox3_camera_driver.cpp
index c7a68c925581b7872e183ca676403394109412bd..5c536274861a5332382efdaa1b8a340f879f5155 100644
--- a/src/mvbluefox3_camera_driver.cpp
+++ b/src/mvbluefox3_camera_driver.cpp
@@ -7,26 +7,44 @@ Mvbluefox3CameraDriver::Mvbluefox3CameraDriver(void)
Mvbluefox3CameraDriver::~Mvbluefox3CameraDriver(void)
{
- DelPtr(this->vcam_ptr);
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ DelPtr(this->vcam_ptr[ii]);
}
void Mvbluefox3CameraDriver::SetObjs(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->vserial.resize(this->num_cams); // Serial numbers.
+ this->vframe_id.resize(this->num_cams); // Frame ids.
}
bool Mvbluefox3CameraDriver::openDriver(void)
{
try
{
- this->vcam_ptr.resize(this->num_cams);
for (int ii = 0; ii < this->num_cams; ++ii)
- this->vcam_ptr[ii] = new CMvbluefox3::CMvbluefox3(this->vserial[ii],devMgr);
-
- // this->vcam_ptr = new CMvbluefox3::CMvbluefox3(this->serial,devMgr);
- std::cout << "[mvBlueFOX3]: Driver opened." << std::endl;
+ {
+ 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 (devMgr.getDeviceBySerial(this->vserial[ii]) == 0)
+ {
+ std::cout << "No device found! Unable to continue. " << std::endl;
+ }
+ else
+ {
+ // create an interface to the found device
+ this->vcam_ptr[ii] = new CMvbluefox3::CMvbluefox3(devMgr.getDeviceBySerial(this->vserial[ii]));
+ }
+ }
+ else
+ std::cout << "Device cannot be found because serial number is not specified." << std::endl;
+ }
+ std::cout << "[mvBlueFOX3]: All drivers opened." << std::endl;
}catch (CMvbluefox3::CmvBlueFOX3Exception& e)
{
std::cout << e.what() << std::endl;
@@ -199,7 +217,7 @@ void Mvbluefox3CameraDriver::GetROSImage(const int &ncam, sensor_msgs::Image &im
CMvbluefox3::codings_str[this->vparams[ncam].pixel_format],
this->vparams[ncam].height,
this->vparams[ncam].width,
- this->vcam_ptr->GetImgLinePitch(),
+ this->vcam_ptr[ncam]->GetImgLinePitch(),
image);
double req_dur_sec = (double)(this->vcam_ptr[ncam]->ReqExposeUs())*1e-6;
diff --git a/src/mvbluefox3_camera_driver_node.cpp b/src/mvbluefox3_camera_driver_node.cpp
index bcdd7284c7e652c1542e1acae8a77991e5876855..c764442e751a0bb94683af261e072b40c35d138c 100644
--- a/src/mvbluefox3_camera_driver_node.cpp
+++ b/src/mvbluefox3_camera_driver_node.cpp
@@ -1,6 +1,4 @@
#include "mvbluefox3_camera_driver_node.h"
-#include <stringstream>
-
Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
iri_base_driver::IriBaseNodeDriver<Mvbluefox3CameraDriver>(nh),
@@ -13,48 +11,48 @@ Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
// [init publishers]
this->driver_.lock();
- this->public_node_handle_.param<std::string>("num_cams", this->driver_.num_cams, 1);
+ this->public_node_handle_.param<int>("num_cams", this->driver_.num_cams, 1);
// Resize all objects
this->driver_.SetObjs();
+
this->camera_manager.resize(this->driver_.num_cams);
this->image_publisher_.resize(this->driver_.num_cams);
this->image_msg_.resize(this->driver_.num_cams);
-
for (int ii = 0; ii < this->driver_.num_cams; ++ii)
{
std::stringstream ss;
- ss << "cam" << ii;
+ ss << "cam" << ii+1;
- std::string cam_mgr_name = "~" + ss.str + std::string("/image_raw");
+ std::string cam_mgr_name = "~" + ss.str() + std::string("/image_raw");
this->camera_manager[ii] = new camera_info_manager::CameraInfoManager(cam_mgr_name);
- std::string img_name = ss.str + "/image_raw";
+ std::string img_name = ss.str() + "/image_raw";
this->image_publisher_[ii] = this->it.advertiseCamera(img_name, 1);
std::string frame_id, tf_prefix;
- std::string frame_id_name = ss.str + "/frame_id";
- std::string tf_prefix_name = ss.str + "/tf_prefix";
- std::string serial_name = ss.str + "/serial";
+ std::string frame_id_name = ss.str() + "/frame_id";
+ std::string tf_prefix_name = ss.str() + "/tf_prefix";
+ std::string serial_name = ss.str() + "/serial";
this->public_node_handle_.param<std::string>(frame_id_name, frame_id, "");
this->public_node_handle_.param<std::string>(tf_prefix_name, tf_prefix, "");
- this->driver_.frame_id[ii] = tf_prefix + "/" + frame_id;
- this->public_node_handle_.param<std::string>(serial_name, this->driver_.serial[ii], "");
+ this->driver_.vframe_id[ii] = tf_prefix + "/" + frame_id;
+ this->public_node_handle_.param<std::string>(serial_name, this->driver_.vserial[ii], "");
this->driver_.unlock();
std::string image_cal_file;
- std::string image_cal_file_name = ss.str + "/calibration_file";
+ std::string image_cal_file_name = ss.str() + "/calibration_file";
public_node_handle_.param<std::string>(image_cal_file_name,image_cal_file,"");
if(this->camera_manager[ii]->validateURL(image_cal_file))
{
if(!this->camera_manager[ii]->loadCameraInfo(image_cal_file))
- ROS_INFO("Invalid calibration file for CAM:%d",ii);
+ ROS_INFO("Invalid calibration file for CAM:%d",ii+1);
}
else
- ROS_INFO("Invalid calibration file for CAM:%d",ii);
+ ROS_INFO("Invalid calibration file for CAM:%d",ii+1);
}
// this->public_node_handle_.param<std::string>("frame_id", frame_id, "");
@@ -148,7 +146,7 @@ void Mvbluefox3CameraDriverNode::reconfigureNodeHook(int level)
Mvbluefox3CameraDriverNode::~Mvbluefox3CameraDriverNode(void)
{
for (int ii = 0; ii < this->driver_.num_cams; ++ii)
- DelPtr(this->camera_manager[ii]);
+ this->driver_.DelPtr(this->camera_manager[ii]);
// [free dynamic memory]
}