diff --git a/README.md b/README.md
index 1b82547..333b321 100644
--- a/README.md
+++ b/README.md
@@ -108,6 +108,38 @@ The solution is to start with a base of ROS time, and to accumulate the dt's fro
To accommodate the difference in clock rates, a PID controller gently pulls the result toward
ROS time.
+### Manually Setting White Balance
+
+Typically the cameras support three different modes for auto white balancing, namely ```Continuous```,
+```Once```, and ```Off```. These can be set via the launch parameter and feature name ```BalanceWhiteAuto```. As the names suggest, the first mode will continuously adjust the white
+balance, while the second mode will measure the white balance once and then freeze the ratio
+parameters. In case, of the third mode, the ratio of the different color channels can and need to be
+ set manually.
+
+To manually set the white balance, camera_aravis provides a couple of launch parameters:
+
+- ```wb_ratio_selector_feature```: Feature name on the camera device to select the color channel for which the ratio is to be set.
+ - Type: ```string```
+ - Default: ```""```
+
+- ```wb_ratio_selectors```: Comma separated list of channel names for which a ratio is to be set.
+The values of this list are set to the selector given by 'wb_ratio_selector_feature' before the
+actual ratios ('wb_ratios') are set. This list should be the same size as the ratio list given in 'wb_ratios'.
+ - Type: ```string```
+ - Default: ```""```
+ - Example: ```"Red,Blue"```
+
+- ```wb_ratio_feature```: Feature name on the camera device to set the ratio for the color channel
+selected by 'wb_ratio_selector_feature'.
+ - Type: ```string```
+ - Default: ```""```
+
+- ```wb_ratios```: Comma separated list of ratios (float) which are to be set for the channels
+specified in 'wb_ratio_selectors'. This list should be the same size as the ratio list given in 'wb_ratios'.
+ - Type: ```string```
+ - Default: ```""```
+ - Example: ```"1.4,2.5"```
+
### Activating PTP Timestamp
Some cameras support the use of the Precision Time Protocol (PTP) to set the timestamps of the
@@ -157,6 +189,57 @@ For each feature a key-value pair is constructed and published in the ```data```
message stated above. If a feature as a list of selectors, one key-value pair is constructed for
each Feature-Selector pair.
+## Ensuring a respawn after failure
+
+Continuously respawning a ROS node:
+
+A node within a launch file can be configured to be automatically respawned in case of failure.
+To do so an additional attribute ```respawn="true"``` is to be set.
+When running camera_aravis as node this can be directly set for the corresponding node, as shown
+below:
+
+```XML
+
+ ...
+
+```
+
+Continuously respawning a ROS nodelet:
+
+In case of using camera_aravis as a nodelet withing a nodelet manager, the actual launch file needs
+to be called from within a simple launch script which, in turn, is called as a respawning node in
+another launch file.
+This is exemplarily demonstrated with [launch_script.sh](scripts/launch_script.sh) and
+[respawning_camera_aravis.launch](launch/respawning_camera_aravis.launch) and shown below:
+
+*launch_script.sh*
+```bash
+#!/bin/bash
+
+roslaunch "$@"
+
+exit
+```
+*respawning_camera_aravis.launch*
+```XML
+
+
+
+
+```
+
+In this, the actual launch file, i.e. ```camera_aravis.launch```, is passed as argument to the
+respawning launch script in ```respawning_camera_aravis.launch```.
+Furthermore, in order for the nodelet manager to finish on crash, it is important the attribute
+```required="true"``` is set to the manager.
+
+**NOTE:** In some cases it is necessary that the shutdown of the node/nodelet is delayed by some
+time in order for the camera to be properly disconnected.
+The shutdown delay time in secondes can by configured by the parameter ```shutdown_delay_s```,
+default: 5 seconds.
+
## Known Issues
### Slow read of white balance and black level values
diff --git a/include/camera_aravis/camera_aravis_nodelet.h b/include/camera_aravis/camera_aravis_nodelet.h
index 663c130..bf5a6b2 100644
--- a/include/camera_aravis/camera_aravis_nodelet.h
+++ b/include/camera_aravis/camera_aravis_nodelet.h
@@ -192,6 +192,8 @@ class CameraAravisNodelet : public nodelet::Nodelet
// integers are integers, doubles are doubles, etc.
void writeCameraFeaturesFromRosparam();
+ void onShutdownTriggered(const ros::TimerEvent&);
+
std::unique_ptr > reconfigure_server_;
boost::recursive_mutex reconfigure_mutex_;
@@ -233,6 +235,9 @@ class CameraAravisNodelet : public nodelet::Nodelet
std::unordered_map implemented_features_;
+ ros::Timer shutdown_trigger_;
+ double shutdown_delay_s_;
+
struct
{
int32_t x = 0;
diff --git a/launch/camera_aravis.launch b/launch/camera_aravis.launch
index bb42a02..0243d4a 100644
--- a/launch/camera_aravis.launch
+++ b/launch/camera_aravis.launch
@@ -17,7 +17,7 @@
-
+
@@ -43,7 +43,13 @@
-
+
+
+
+
+
+
+
diff --git a/launch/respawning_camera_aravis.launch b/launch/respawning_camera_aravis.launch
new file mode 100644
index 0000000..a084114
--- /dev/null
+++ b/launch/respawning_camera_aravis.launch
@@ -0,0 +1,6 @@
+
+
+
+
diff --git a/scripts/launch_script.sh b/scripts/launch_script.sh
new file mode 100755
index 0000000..5bf1d8d
--- /dev/null
+++ b/scripts/launch_script.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+roslaunch "$@"
+
+exit
diff --git a/src/camera_aravis_nodelet.cpp b/src/camera_aravis_nodelet.cpp
index d7db8d8..8c29582 100644
--- a/src/camera_aravis_nodelet.cpp
+++ b/src/camera_aravis_nodelet.cpp
@@ -100,6 +100,7 @@ CameraAravisNodelet::~CameraAravisNodelet()
void CameraAravisNodelet::onInit()
{
+ ros::NodeHandle nh = getNodeHandle();
ros::NodeHandle pnh = getPrivateNodeHandle();
// Retrieve ros parameters
@@ -112,6 +113,24 @@ void CameraAravisNodelet::onInit()
ptp_set_cmd_feature_ = pnh.param("ptp_set_cmd_feature_name", ptp_set_cmd_feature_);
bool init_params_from_reconfig = pnh.param("init_params_from_dyn_reconfigure", true);
+
+ std::string awb_mode = pnh.param("BalanceWhiteAuto", "Continuous");
+ std::string wb_ratio_selector_feature = pnh.param("wb_ratio_selector_feature", "");
+ std::string wb_ratio_feature = pnh.param("wb_ratio_feature", "");
+
+ std::string wb_ratio_selector_args = pnh.param("wb_ratio_selectors", "");
+ std::vector wb_ratio_selector_values;
+ parseStringArgs(wb_ratio_selector_args, wb_ratio_selector_values);
+
+ std::string wb_ratio_args = pnh.param("wb_ratios", "");
+ std::vector wb_ratio_str_values;
+ parseStringArgs(wb_ratio_args, wb_ratio_str_values);
+
+ if(wb_ratio_selector_values.size() != wb_ratio_str_values.size())
+ {
+ ROS_WARN("Lists of 'wb_ratio_selector' and 'wb_ratio' have different sizes. "
+ "Only setting values which exist in both lists.");
+ }
pub_ext_camera_info_ = pnh.param("ExtendedCameraInfo", pub_ext_camera_info_); // publish an extended camera info message
pub_tf_optical_ = pnh.param("publish_tf", pub_tf_optical_); // should we publish tf transforms to camera optical frame?
@@ -137,6 +156,10 @@ void CameraAravisNodelet::onInit()
diagnostic_publish_rate_ =
std::max(0.0, pnh.param("diagnostic_publish_rate", 0.1));
+ shutdown_trigger_ = nh.createTimer(ros::Duration(0.1), &CameraAravisNodelet::onShutdownTriggered,
+ this, true, false);
+ shutdown_delay_s_ = pnh.param("shutdown_delay_s", 5.0);
+
// Print out some useful info.
ROS_INFO("Attached cameras:");
arv_update_device_list();
@@ -271,6 +294,20 @@ void CameraAravisNodelet::onInit()
// possibly set or override from given parameter
writeCameraFeaturesFromRosparam();
+
+ if(awb_mode == "Off"
+ && !wb_ratio_selector_feature.empty() && implemented_features_[wb_ratio_selector_feature]
+ && !wb_ratio_feature.empty() && implemented_features_[wb_ratio_feature])
+ {
+ for(int l = 0; l < std::min(wb_ratio_selector_values.size(), wb_ratio_str_values.size());
+ l++)
+ {
+ arv_device_set_string_feature_value(p_device_, wb_ratio_selector_feature.c_str(),
+ wb_ratio_selector_values[l].c_str());
+ arv_device_set_float_feature_value(p_device_, wb_ratio_feature.c_str(),
+ std::stof(wb_ratio_str_values[l]));
+ }
+ }
}
ros::Duration(2.0).sleep();
@@ -460,16 +497,13 @@ void CameraAravisNodelet::onInit()
ROS_INFO(" ROI x,y,w,h = %d, %d, %d, %d", roi_.x, roi_.y, roi_.width, roi_.height);
ROS_INFO(" Pixel format = %s", sensors_[0]->pixel_format.c_str());
ROS_INFO(" BitsPerPixel = %lu", sensors_[0]->n_bits_pixel);
- ROS_INFO(
- " Acquisition Mode = %s",
+ ROS_INFO(" Acquisition Mode = %s",
implemented_features_["AcquisitionMode"] ? arv_device_get_string_feature_value(p_device_, "AcquisitionMode") :
"(not implemented in camera)");
- ROS_INFO(
- " Trigger Mode = %s",
+ ROS_INFO(" Trigger Mode = %s",
implemented_features_["TriggerMode"] ? arv_device_get_string_feature_value(p_device_, "TriggerMode") :
"(not implemented in camera)");
- ROS_INFO(
- " Trigger Source = %s",
+ ROS_INFO(" Trigger Source = %s",
implemented_features_["TriggerSource"] ? arv_device_get_string_feature_value(p_device_, "TriggerSource") :
"(not implemented in camera)");
ROS_INFO(" Can set FrameRate: %s", implemented_features_["AcquisitionFrameRate"] ? "True" : "False");
@@ -478,6 +512,24 @@ void CameraAravisNodelet::onInit()
ROS_INFO(" AcquisitionFrameRate = %g hz", config_.AcquisitionFrameRate);
}
+ if (implemented_features_["BalanceWhiteAuto"])
+ {
+ ROS_INFO(" BalanceWhiteAuto = %s", awb_mode.c_str());
+ if(awb_mode != "Continuous"
+ && !wb_ratio_selector_feature.empty() && implemented_features_[wb_ratio_selector_feature]
+ && !wb_ratio_feature.empty() && implemented_features_[wb_ratio_feature])
+ {
+ for(int l = 0; l < std::min(wb_ratio_selector_values.size(), wb_ratio_str_values.size());
+ l++)
+ {
+ arv_device_set_string_feature_value(p_device_, wb_ratio_selector_feature.c_str(),
+ wb_ratio_selector_values[l].c_str());
+ float wb_ratio_val = arv_device_get_float_feature_value(p_device_, wb_ratio_feature.c_str());
+ ROS_INFO(" BalanceRatio %s = %f", wb_ratio_selector_values[l].c_str(), wb_ratio_val);
+ }
+ }
+ }
+
ROS_INFO(" Can set Exposure: %s", implemented_features_["ExposureTime"] ? "True" : "False");
if (implemented_features_["ExposureTime"])
{
@@ -502,8 +554,21 @@ void CameraAravisNodelet::onInit()
// Reset PTP clock
if (use_ptp_stamp_)
+ {
resetPtpClock();
+ if(!ptp_set_cmd_feature_.empty())
+ {
+ if(implemented_features_[ptp_set_cmd_feature_])
+ {
+ arv_device_execute_command(p_device_, ptp_set_cmd_feature_.c_str());
+ }
+ else
+ ROS_WARN("PTP Error: ptp_set_cmd_feature_ '%s' is not available on the device.",
+ ptp_set_cmd_feature_.c_str());
+ }
+ }
+
// spawn camera stream in thread, so onInit() is not blocked
spawning_ = true;
spawn_stream_thread_ = std::thread(&CameraAravisNodelet::spawnStream, this);
@@ -709,17 +774,7 @@ void CameraAravisNodelet::resetPtpClock()
arv_device_set_boolean_feature_value(p_device_, ptp_enable_feature_.c_str(), false);
arv_device_set_boolean_feature_value(p_device_, ptp_enable_feature_.c_str(), true);
-
- if(!ptp_set_cmd_feature_.empty())
- {
- if(implemented_features_[ptp_set_cmd_feature_])
- arv_device_execute_command(p_device_, ptp_set_cmd_feature_.c_str());
- else
- ROS_WARN("PTP Error: ptp_set_cmd_feature_ '%s' is not available on the device.",
- ptp_set_cmd_feature_.c_str());
- }
}
-
}
void CameraAravisNodelet::cameraAutoInfoCallback(const CameraAutoInfoConstPtr &msg_ptr)
@@ -1444,13 +1499,11 @@ void CameraAravisNodelet::fillExtendedCameraInfoMessage(ExtendedCameraInfo &msg)
void CameraAravisNodelet::controlLostCallback(ArvDevice *p_gv_device, gpointer can_instance)
{
CameraAravisNodelet *p_can = (CameraAravisNodelet*)can_instance;
- ROS_ERROR("Control to aravis device lost.");
- nodelet::NodeletUnload unload_service;
- unload_service.request.name = p_can->getName();
- if (false == ros::service::call(ros::this_node::getName() + "/unload_nodelet", unload_service))
- {
- ros::shutdown();
- }
+ ROS_ERROR("Control to aravis device lost.\n\t> Nodelet name: %s."
+ "\n\t> Shutting down. Allowing for respawn.",
+ p_can->getName().c_str());
+
+ p_can->shutdown_trigger_.start();
}
void CameraAravisNodelet::softwareTriggerLoop()
@@ -1810,4 +1863,17 @@ void CameraAravisNodelet::writeCameraFeaturesFromRosparam()
}
}
+void CameraAravisNodelet::onShutdownTriggered(const ros::TimerEvent&)
+{
+ nodelet::NodeletUnload unload_service;
+ unload_service.request.name = this->getName();
+ ros::service::call(this->getName() + "/unload_nodelet", unload_service);
+ ROS_INFO("Nodelet unloaded.");
+
+ ros::Duration(shutdown_delay_s_).sleep();
+
+ ros::shutdown();
+ ROS_INFO("Shut down successful.");
+}
+
} // end namespace camera_aravis