diff --git a/CMakeLists.txt b/CMakeLists.txt
index 68d5c73fdbab73e2e4d0d6a16156ab78ab5e8e72..cb9073c717b7a52c6835c7a72461551787a7ff4f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,8 +10,8 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver sensor_msgs image_transp
# find_package(catkin REQUIRED COMPONENTS iri_base_driver cv_bridge image_transport camera_info_manager sensor_msgs)
find_package(mvbluefox3 REQUIRED)
find_package(mvIMPACT REQUIRED)
-# FIND_PACKAGE(OpenCV 3 REQUIRED)
-# find_package(OpenCV 2.4 REQUIRED)
+#find_package(OpenCV 3 REQUIRED)
+#find_package(OpenCV 2.4 REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -71,9 +71,11 @@ catkin_package(
# Add system and labrobotica run time dependencies here
# ********************************************************************
DEPENDS mvbluefox3 mvIMPACT
- # DEPENDS mvbluefox3 mvIMPACT OpenCV
+# DEPENDS mvbluefox3 mvIMPACT OpenCV
)
+#MESSAGE(${OpenCV_VERSION})
+
###########
## Build ##
###########
@@ -85,7 +87,7 @@ 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(${OpenCV_INCLUDE_DIRS})
# include_directories(${<dependency>_INCLUDE_DIR})
## Declare a cpp library
@@ -100,7 +102,7 @@ add_executable(${PROJECT_NAME} src/mvbluefox3_camera_driver.cpp src/mvbluefox3_c
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_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES})
# 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 2c4e198b486123825998aefb84b9869df078a403..d1f664ef0070cc4e7e3f0e4f2bceb9a0b6d5364c 100644
--- a/include/mvbluefox3_camera_driver.h
+++ b/include/mvbluefox3_camera_driver.h
@@ -57,9 +57,11 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
private:
// private attributes and methods
- CMvbluefox3::CMvbluefox3* cam_ptr; // Camera pointer.
+ mvIMPACT::acquire::DeviceManager devMgr; // Device Manager.
- CMvbluefox3::CParams params; // Camera parameters.
+ std::vector<CMvbluefox3::CMvbluefox3*> vcam_ptr; // Camera pointer.
+
+ std::vector<CMvbluefox3::CParams> vparams; // Camera parameters.
/**
* \brief Delete pointer
@@ -79,7 +81,7 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
/**
* \brief Set Configuration
*
- * Set Configuration with params_ object (to be filled externally).
+ * Set Configuration with vparams_ object (to be filled externally).
*/
void SetConfig(void);
@@ -106,8 +108,9 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
public:
- std::string serial; // Camera serial number.
- std::string frame_id; // Image frame id.
+ int num_cams; // Number of cameras.
+ std::vector<std::string> vserial; // Serial numbers.
+ std::vector<std::string> vframe_id; // Frame ids.
/**
* \brief define config type
diff --git a/include/mvbluefox3_camera_driver_node.h b/include/mvbluefox3_camera_driver_node.h
index 30756e83677d619dae98c6c5f0a168d6d757599f..374cd7f13d8a0e78f9054d7a5a8ec056bcba0c12 100644
--- a/include/mvbluefox3_camera_driver_node.h
+++ b/include/mvbluefox3_camera_driver_node.h
@@ -66,9 +66,9 @@ class Mvbluefox3CameraDriverNode : public iri_base_driver::IriBaseNodeDriver<Mvb
//cv_bridge::CvImagePtr cv_image_;
image_transport::ImageTransport it;
- camera_info_manager::CameraInfoManager camera_manager;
- image_transport::CameraPublisher image_publisher_;
- 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_;
// [subscriber attributes]
diff --git a/launch/iri_mvbluefox3_camera.launch b/launch/iri_mvbluefox3_camera.launch
index 9db2688349ac816346da98613757ca61f7685906..ecea73c46c314be3245f58f90563b98a8b3f868d 100644
--- a/launch/iri_mvbluefox3_camera.launch
+++ b/launch/iri_mvbluefox3_camera.launch
@@ -1,4 +1,4 @@
-<!-- -->
+<!DOCTYPE html>
<launch>
<node pkg="iri_mvbluefox3_camera"
@@ -12,6 +12,17 @@
<rosparam command="load" file="$(find iri_mvbluefox3_camera)/params/F0300141_params.yaml" />
</node>
+<!-- <node pkg="iri_mvbluefox3_camera"
+ name="iri_mvbluefox3_camera2"
+ type="iri_mvbluefox3_camera"
+ output="screen">
+ <param name="serial" value="F0300123"/>
+ <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"/>
+ <rosparam command="load" file="$(find iri_mvbluefox3_camera)/params/F0300141_params.yaml" />
+ </node> -->
+
<!-- <node pkg="image_view"
type="image_view"
name="image_view" >
diff --git a/src/mvbluefox3_camera_driver.cpp b/src/mvbluefox3_camera_driver.cpp
index 2bbc6b5ec2f91ffafc26adba95c8512f97eb1456..c7a68c925581b7872e183ca676403394109412bd 100644
--- a/src/mvbluefox3_camera_driver.cpp
+++ b/src/mvbluefox3_camera_driver.cpp
@@ -2,18 +2,30 @@
Mvbluefox3CameraDriver::Mvbluefox3CameraDriver(void)
{
+ this->num_cams = 1;
}
Mvbluefox3CameraDriver::~Mvbluefox3CameraDriver(void)
{
- DelPtr(this->cam_ptr);
+ DelPtr(this->vcam_ptr);
+}
+
+void Mvbluefox3CameraDriver::SetObjs(void)
+{
+ 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.
}
bool Mvbluefox3CameraDriver::openDriver(void)
{
try
{
- this->cam_ptr = new CMvbluefox3::CMvbluefox3(this->serial);
+ 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;
}catch (CMvbluefox3::CmvBlueFOX3Exception& e)
{
@@ -26,7 +38,9 @@ bool Mvbluefox3CameraDriver::closeDriver(void)
{
try
{
- this->cam_ptr->CloseDevice();
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ this->vcam_ptr[ii]->CloseDevice();
+ // this->vcam_ptr->CloseDevice();
std::cout << "[mvBlueFOX3]: Driver closed." << std::endl;
}catch (CMvbluefox3::CmvBlueFOX3Exception& e)
{
@@ -49,24 +63,26 @@ void Mvbluefox3CameraDriver::config_update(Config& new_cfg, uint32_t level)
{
this->lock();
+ // TODO:
+
// depending on current state
// update driver with new_cfg data
switch(this->getState())
{
case Mvbluefox3CameraDriver::CLOSED:
- // Fill initial parameters
- this->params = DynRecToParams(new_cfg);
+ // // Fill initial parameters
+ // this->vparams = DynRecToParams(new_cfg);
break;
case Mvbluefox3CameraDriver::OPENED:
- // Fill parameters
- this->params = DynRecToParams(new_cfg);
- // Set new config is requested
- if (new_cfg.apply_cfg)
- {
- SetConfig();
- new_cfg.apply_cfg = false;
- }
+ // // Fill parameters
+ // this->vparams = DynRecToParams(new_cfg);
+ // // Set new config is requested
+ // if (new_cfg.apply_cfg)
+ // {
+ // SetConfig();
+ // new_cfg.apply_cfg = false;
+ // }
break;
case Mvbluefox3CameraDriver::RUNNING:
@@ -138,40 +154,57 @@ CMvbluefox3::codings_t Mvbluefox3CameraDriver::PfToCodes(int &pf)
return ct;
}
-void Mvbluefox3CameraDriver::SetConfig(void)
+void Mvbluefox3CameraDriver::SetConfig(const int &ncam)
{
try
{
- this->cam_ptr->SetConfig(this->params);
- std::cout << "[mvBlueFOX3]: New configuration set." << std::endl;
+ this->vcam_ptr[ncam]->SetConfig(this->vparams[ncam]);
+ std::cout << "[mvBlueFOX3]: New configuration set for CAM: " << ncam << "." << std::endl;
+ // this->vcam_ptr->SetConfig(this->vparams);
+ // std::cout << "[mvBlueFOX3]: New configuration set." << std::endl;
}catch (CMvbluefox3::CmvBlueFOX3Exception& e)
{
std::cout << e.what() << std::endl;
}
}
-void Mvbluefox3CameraDriver::GetConfig(void)
+void Mvbluefox3CameraDriver::GetConfig(const int &ncam)
{
- this->params = this->cam_ptr->GetConfig();
+ this->vparams[ncam] = this->vcam_ptr[ncam]->GetConfig();
+ // this->vparams = this->vcam_ptr->GetConfig();
}
-void Mvbluefox3CameraDriver::GetROSImage(sensor_msgs::Image &img_msg)
+void Mvbluefox3CameraDriver::GetROSImage(const int &ncam, sensor_msgs::Image &img_msg)
{
ros::Time start_time = ros::Time::now();
+ // char *image;
+ // this->vcam_ptr->GetImage(&image);
+
+ // sensor_msgs::fillImage(img_msg,
+ // CMvbluefox3::codings_str[this->vparams.pixel_format],
+ // this->vparams.height,
+ // this->vparams.width,
+ // this->vcam_ptr->GetImgLinePitch(),
+ // image);
+
+ // double req_dur_sec = (double)(this->vcam_ptr->ReqExposeUs())*1e-6;
+ // img_msg.header.stamp = start_time + ros::Duration(req_dur_sec/2.0);
+ // img_msg.header.frame_id = this->frame_id;
+
char *image;
- this->cam_ptr->GetImage(&image);
+ this->vcam_ptr[ncam]->GetImage(&image);
sensor_msgs::fillImage(img_msg,
- CMvbluefox3::codings_str[this->params.pixel_format],
- this->params.height,
- this->params.width,
- this->cam_ptr->GetImgLinePitch(),
+ CMvbluefox3::codings_str[this->vparams[ncam].pixel_format],
+ this->vparams[ncam].height,
+ this->vparams[ncam].width,
+ this->vcam_ptr->GetImgLinePitch(),
image);
- double req_dur_sec = (double)(this->cam_ptr->ReqExposeUs())*1e-6;
+ 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->frame_id;
+ img_msg.header.frame_id = this->vframe_id[ncam];
delete [] image;
}
diff --git a/src/mvbluefox3_camera_driver_node.cpp b/src/mvbluefox3_camera_driver_node.cpp
index efa9b8a888fb2f3475227ffc823df4549b35e722..bcdd7284c7e652c1542e1acae8a77991e5876855 100644
--- a/src/mvbluefox3_camera_driver_node.cpp
+++ b/src/mvbluefox3_camera_driver_node.cpp
@@ -1,34 +1,79 @@
#include "mvbluefox3_camera_driver_node.h"
+#include <stringstream>
+
Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
iri_base_driver::IriBaseNodeDriver<Mvbluefox3CameraDriver>(nh),
- camera_manager(ros::NodeHandle("~image_raw")),
+ // camera_manager(ros::NodeHandle("~image_raw")),
it(this->public_node_handle_)
{
//init class attributes if necessary
this->loop_rate_ = 50;//in [Hz]
// [init publishers]
- this->image_publisher_ = this->it.advertiseCamera("image_raw", 1);
this->driver_.lock();
- std::string frame_id, tf_prefix;
- this->public_node_handle_.param<std::string>("frame_id", frame_id, "");
- this->public_node_handle_.param<std::string>("tf_prefix", tf_prefix, "");
- this->driver_.frame_id = tf_prefix + "/" + frame_id;
- this->public_node_handle_.param<std::string>("serial", this->driver_.serial, "");
- this->driver_.unlock();
+ this->public_node_handle_.param<std::string>("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);
- std::string image_cal_file;
- public_node_handle_.param<std::string>("calibration_file",image_cal_file,"");
- if(this->camera_manager.validateURL(image_cal_file))
+ for (int ii = 0; ii < this->driver_.num_cams; ++ii)
{
- if(!this->camera_manager.loadCameraInfo(image_cal_file))
- ROS_INFO("Invalid calibration file");
+ std::stringstream ss;
+ ss << "cam" << ii;
+
+ 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";
+ 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";
+ 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_.unlock();
+
+ std::string image_cal_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);
+ }
+ else
+ ROS_INFO("Invalid calibration file for CAM:%d",ii);
}
- else
- ROS_INFO("Invalid calibration file");
+
+ // this->public_node_handle_.param<std::string>("frame_id", frame_id, "");
+ // this->public_node_handle_.param<std::string>("tf_prefix", tf_prefix, "");
+ // this->driver_.frame_id = tf_prefix + "/" + frame_id;
+ // this->public_node_handle_.param<std::string>("serial", this->driver_.serial, "");
+
+ // this->driver_.unlock();
+
+ // std::string image_cal_file;
+ // public_node_handle_.param<std::string>("calibration_file",image_cal_file,"");
+
+ // if(this->camera_manager.validateURL(image_cal_file))
+ // {
+ // if(!this->camera_manager.loadCameraInfo(image_cal_file))
+ // ROS_INFO("Invalid calibration file");
+ // }
+ // else
+ // ROS_INFO("Invalid calibration file");
// [init subscribers]
@@ -48,23 +93,24 @@ void Mvbluefox3CameraDriverNode::mainNodeThread(void)
// [fill msg structures]
// Image
- this->driver_.lock();
- this->driver_.GetROSImage(this->image_msg_);
- this->driver_.unlock();
+ for (int ii = 0; ii < this->driver_.num_cams; ++ii)
+ {
+ this->driver_.lock();
+ this->driver_.GetROSImage(ii,this->image_msg_[ii]);
+ this->driver_.unlock();
+
+ // Camera info
+ sensor_msgs::CameraInfo camera_info = this->camera_manager[ii]->getCameraInfo();
+ camera_info.header = this->image_msg_[ii].header;
+ this->image_publisher_[ii].publish(this->image_msg_[ii],camera_info);
+ }
- // Camera info
- sensor_msgs::CameraInfo camera_info = this->camera_manager.getCameraInfo();
- camera_info.header = this->image_msg_.header;
-
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]
// [publish messages]
- // Uncomment the following line to convert an OpenCV image to a ROS image message
- //this->image_msg_=*this->cv_image_->toImageMsg();
- // Uncomment the following line to publish the image together with the camera information
- this->image_publisher_.publish(this->image_msg_,camera_info);
+
}
/* [subscriber callbacks] */
@@ -101,6 +147,8 @@ void Mvbluefox3CameraDriverNode::reconfigureNodeHook(int level)
Mvbluefox3CameraDriverNode::~Mvbluefox3CameraDriverNode(void)
{
+ for (int ii = 0; ii < this->driver_.num_cams; ++ii)
+ DelPtr(this->camera_manager[ii]);
// [free dynamic memory]
}