From 6fc0a5ae92050a4c81aed94a85ab18b0689f62da Mon Sep 17 00:00:00 2001
From: Angel Santamaria-Navarro <asantamaria@iri.upc.edu>
Date: Thu, 15 Dec 2016 12:25:43 +0100
Subject: [PATCH] all done to debug dynamic reconfigure parameters
---
cfg/Mvbluefox3Camera.cfg | 2 +
include/mvbluefox3_camera_driver.h | 23 ++++-
launch/iri_mvbluefox3_camera.launch | 36 ++++----
src/mvbluefox3_camera_driver.cpp | 117 +++++++++++++++++++-------
src/mvbluefox3_camera_driver_node.cpp | 36 +++-----
5 files changed, 132 insertions(+), 82 deletions(-)
diff --git a/cfg/Mvbluefox3Camera.cfg b/cfg/Mvbluefox3Camera.cfg
index 6375edf..0dc19fe 100755
--- a/cfg/Mvbluefox3Camera.cfg
+++ b/cfg/Mvbluefox3Camera.cfg
@@ -68,6 +68,8 @@ gen.const("BGR8", int_t, 5, "BGR coding with 8 bits per channel"),
gen.const("YUV422", int_t, 6, "YUV coding with 4,2,2 pixels per channel"),
gen.const("YUV444", int_t, 7, "YUV coding with 4,4,4 pixels per channel")],"Available color modes")
+gen.add("cam_name", str_t, SensorLevels.RECONFIGURE_STOP, "Camera number to apply settings", "cam1")
+
gen.add("apply_cfg", bool_t, SensorLevels.RECONFIGURE_STOP, "Apply new configuration", False)
gen.add("pixel_format", int_t, SensorLevels.RECONFIGURE_STOP, "Color modes", 1, 0, 7, edit_method=enum_color_coding)
diff --git a/include/mvbluefox3_camera_driver.h b/include/mvbluefox3_camera_driver.h
index 1389be0..41a6ee1 100644
--- a/include/mvbluefox3_camera_driver.h
+++ b/include/mvbluefox3_camera_driver.h
@@ -84,6 +84,13 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
*/
CMvbluefox3::CParams DynRecToParams(iri_mvbluefox3_camera::Mvbluefox3CameraConfig &cfg);
+ /**
+ * \brief Camera parameters to dynamic reconfigure
+ *
+ * Camera parameters to dynamic reconfigure.
+ */
+ iri_mvbluefox3_camera::Mvbluefox3CameraConfig ParamsToDynRec(CMvbluefox3::CParams ¶ms);
+
/**
* \brief Conversion from pixel format to codings_t
*
@@ -91,11 +98,19 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
*/
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);
+
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; // Camera names.
/**
* \brief define config type
@@ -193,12 +208,12 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
void GetROSImage(const int &ncam, sensor_msgs::Image &img_msg);
/**
- * \brief Set objects in driver class
+ * \brief Resize objects in driver class
*
- * Set objects in driver class.
+ * Resize objects in driver class.
*
*/
- void SetObjs(void);
+ void ResizeObjs(void);
/**
* \brief Delete pointer
@@ -214,7 +229,7 @@ class Mvbluefox3CameraDriver : public iri_base_driver::IriBaseDriver
delete [] ptr;
}
}
-
+
/**
* \brief Destructor
*
diff --git a/launch/iri_mvbluefox3_camera.launch b/launch/iri_mvbluefox3_camera.launch
index 2ebf233..e0f4b27 100644
--- a/launch/iri_mvbluefox3_camera.launch
+++ b/launch/iri_mvbluefox3_camera.launch
@@ -1,34 +1,30 @@
<!DOCTYPE html>
<launch>
+ <arg name="cam1_name" default="cam1" />
+ <arg name="cam2_name" default="cam2" />
+
<node pkg="iri_mvbluefox3_camera"
name="iri_mvbluefox3_camera"
type="iri_mvbluefox3_camera"
output="screen">
- <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"/>
+
+ <param name="num_cams" value="2"/>
+
+ <param name="cam1_name" value="$(arg cam1_name)"/>
+ <param name="$(arg cam1_name)/serial" value="F0300141"/>
+ <param name="$(arg cam1_name)/frame_id" value="mvBlueFOX3_camera"/>
+ <param name="$(arg cam1_name)/tf_preix" value=""/>
+ <param name="$(arg cam1_name)/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"/>
+ <param name="cam2_name" value="$(arg cam2_name)"/>
+ <param name="$(arg cam2_name)/serial" value="F0300123"/>
+ <param name="$(arg cam2_name)/frame_id" value="mvBlueFOX3_camera"/>
+ <param name="$(arg cam2_name)/tf_preix" value=""/>
+ <param name="$(arg cam2_name)/calibration_file" value="$(find iri_mvbluefox3_camera)/params/F0300141_calib.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 5c53627..c80012b 100644
--- a/src/mvbluefox3_camera_driver.cpp
+++ b/src/mvbluefox3_camera_driver.cpp
@@ -11,12 +11,13 @@ Mvbluefox3CameraDriver::~Mvbluefox3CameraDriver(void)
DelPtr(this->vcam_ptr[ii]);
}
-void Mvbluefox3CameraDriver::SetObjs(void)
+void Mvbluefox3CameraDriver::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);
}
bool Mvbluefox3CameraDriver::openDriver(void)
@@ -33,18 +34,17 @@ bool Mvbluefox3CameraDriver::openDriver(void)
if (devMgr.getDeviceBySerial(this->vserial[ii]) == 0)
{
- std::cout << "No device found! Unable to continue. " << std::endl;
+ std::cerr << "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::cerr << "Device cannot be found because serial number is not specified." << std::endl;
}
- std::cout << "[mvBlueFOX3]: All drivers opened." << std::endl;
+ std::cout << "[mvBlueFOX3]: All cameras opened successfully." << std::endl;
}catch (CMvbluefox3::CmvBlueFOX3Exception& e)
{
std::cout << e.what() << std::endl;
@@ -89,18 +89,27 @@ void Mvbluefox3CameraDriver::config_update(Config& new_cfg, uint32_t level)
{
case Mvbluefox3CameraDriver::CLOSED:
// // Fill initial parameters
- // this->vparams = DynRecToParams(new_cfg);
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ this->vparams[ii] = DynRecToParams(new_cfg);
break;
case Mvbluefox3CameraDriver::OPENED:
- // // Fill parameters
- // this->vparams = DynRecToParams(new_cfg);
- // // Set new config is requested
- // if (new_cfg.apply_cfg)
- // {
- // SetConfig();
- // new_cfg.apply_cfg = false;
- // }
+ // Set new config is requested
+ if (new_cfg.apply_cfg)
+ {
+ for (int ii = 0; ii < this->num_cams; ++ii)
+ {
+ if (this->vcam_name[ii].compare(new_cfg.cam_name) == 0)
+ {
+ // Fill parameters
+ this->vparams[ii] = DynRecToParams(new_cfg);
+ SetConfig(ii);
+ GetConfig(ii);
+ new_cfg = ParamsToDynRec(this->vparams[ii]);
+ }
+ }
+ new_cfg.apply_cfg = false;
+ }
break;
case Mvbluefox3CameraDriver::RUNNING:
@@ -155,6 +164,49 @@ CMvbluefox3::CParams Mvbluefox3CameraDriver::DynRecToParams(iri_mvbluefox3_camer
return p;
}
+iri_mvbluefox3_camera::Mvbluefox3CameraConfig Mvbluefox3CameraDriver::ParamsToDynRec(CMvbluefox3::CParams &p)
+{
+ iri_mvbluefox3_camera::Mvbluefox3CameraConfig cfg;
+
+ 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;
+}
+
+
CMvbluefox3::codings_t Mvbluefox3CameraDriver::PfToCodes(int &pf)
{
CMvbluefox3::codings_t ct;
@@ -172,14 +224,30 @@ CMvbluefox3::codings_t Mvbluefox3CameraDriver::PfToCodes(int &pf)
return ct;
}
+int Mvbluefox3CameraDriver::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::rgb8: pf = 3; break;
+ case CMvbluefox3::rgb16: pf = 4; break;
+ case CMvbluefox3::bgr8: pf = 5; break;
+ case CMvbluefox3::yuv422: pf = 6; break;
+ case CMvbluefox3::yuv444: pf = 7; break;
+ }
+ return pf;
+}
+
+
void Mvbluefox3CameraDriver::SetConfig(const int &ncam)
{
try
{
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;
+ std::cout << "[mvBlueFOX3]: New configuration set for " << this->vcam_name[ncam] << "." << std::endl;
}catch (CMvbluefox3::CmvBlueFOX3Exception& e)
{
std::cout << e.what() << std::endl;
@@ -189,27 +257,12 @@ void Mvbluefox3CameraDriver::SetConfig(const int &ncam)
void Mvbluefox3CameraDriver::GetConfig(const int &ncam)
{
this->vparams[ncam] = this->vcam_ptr[ncam]->GetConfig();
- // this->vparams = this->vcam_ptr->GetConfig();
}
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->vcam_ptr[ncam]->GetImage(&image);
diff --git a/src/mvbluefox3_camera_driver_node.cpp b/src/mvbluefox3_camera_driver_node.cpp
index c764442..5e14d86 100644
--- a/src/mvbluefox3_camera_driver_node.cpp
+++ b/src/mvbluefox3_camera_driver_node.cpp
@@ -14,7 +14,7 @@ Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
this->public_node_handle_.param<int>("num_cams", this->driver_.num_cams, 1);
// Resize all objects
- this->driver_.SetObjs();
+ this->driver_.ResizeObjs();
this->camera_manager.resize(this->driver_.num_cams);
this->image_publisher_.resize(this->driver_.num_cams);
@@ -23,18 +23,20 @@ Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
for (int ii = 0; ii < this->driver_.num_cams; ++ii)
{
std::stringstream ss;
- ss << "cam" << ii+1;
+ ss << "cam" << ii+1 << "_name";
- std::string cam_mgr_name = "~" + ss.str() + std::string("/image_raw");
+ this->public_node_handle_.param<std::string>(ss.str(), this->driver_.vcam_name[ii], "cam1");
+
+ std::string cam_mgr_name = "~" + this->driver_.vcam_name[ii] + 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 = this->driver_.vcam_name[ii] + "/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 = this->driver_.vcam_name[ii] + "/frame_id";
+ std::string tf_prefix_name = this->driver_.vcam_name[ii] + "/tf_prefix";
+ std::string serial_name = this->driver_.vcam_name[ii] + "/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_.vframe_id[ii] = tf_prefix + "/" + frame_id;
@@ -43,7 +45,7 @@ Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
this->driver_.unlock();
std::string image_cal_file;
- std::string image_cal_file_name = ss.str() + "/calibration_file";
+ std::string image_cal_file_name = this->driver_.vcam_name[ii] + "/calibration_file";
public_node_handle_.param<std::string>(image_cal_file_name,image_cal_file,"");
if(this->camera_manager[ii]->validateURL(image_cal_file))
@@ -55,24 +57,6 @@ Mvbluefox3CameraDriverNode::Mvbluefox3CameraDriverNode(ros::NodeHandle &nh) :
ROS_INFO("Invalid calibration file for CAM:%d",ii+1);
}
- // 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]
// [init services]
--
GitLab