Merge pull request #30 from FraunhoferIOSB/WbLaunchParameter

Adding support to manually set white balance Launch configuration, scripts and adjustments to allow respawning
FraunhoferIOSB · Jan 31, 2024 · cddb48e · cddb48e
2 parents cfdd85d + d02c39d
commit cddb48e
Show file tree

Hide file tree

Showing 6 changed files with 196 additions and 25 deletions.
diff --git 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
+
+<b>Continuously respawning a ROS node:</b>
+
+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
+<node pkg="camera_aravis" type="cam_aravis" name="camera_aravis_node" respawn="true" output="screen">
+	...
+</node>
+```
+
+<b>Continuously respawning a ROS nodelet:</b>
+
+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
+<?xml version="1.0"?>
+<launch>
+  <node pkg="camera_aravis" type="launch_script.sh" name="respawning_camera_aravis"
+        respawn="true" output="screen"
+        args="$(find camera_aravis)/launch/camera_aravis.launch"/>
+</launch> 
+```
+
+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
@@ -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<dynamic_reconfigure::Server<Config> > reconfigure_server_;
   boost::recursive_mutex reconfigure_mutex_;
 
@@ -233,6 +235,9 @@ class CameraAravisNodelet : public nodelet::Nodelet
 
   std::unordered_map<std::string, const bool> 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
@@ -17,7 +17,7 @@
   <arg name="verbose"                  default="false"/>
 
   <!-- Nodelet Manager -->
-  <node if="$(arg load_manager)" pkg="nodelet" type="nodelet" name="$(arg manager_name)" args="manager" output="screen">
+  <node if="$(arg load_manager)" pkg="nodelet" type="nodelet" name="$(arg manager_name)" required="true" args="manager" output="screen">
     <param name="num_worker_threads" value="$(arg manager_threads)" />
   </node>
 
@@ -43,7 +43,13 @@
       <param name="AutoFunctionsROIPreset" value="AutoFunctionsROIPreset_Full"/>
       <param name="ExposureAuto"         value="Continuous"/>
       <param name="GainAuto"             value="Continuous"/>
-      <param name="BalanceWhiteAuto"     value="Continuous"/>
+
+      <!-- BalanceWhiteAuto Modes: Continuous, Once, Off -->
+      <param name="BalanceWhiteAuto"            value="Off"/>
+      <param name="wb_ratio_selector_feature"	  value="BalanceRatioSelector"/>
+      <param name="wb_ratio_selectors"	        value="Red,Blue"/>
+      <param name="wb_ratio_feature"	  	      value="BalanceRatio"/>
+      <param name="wb_ratios"	  	  	          value="1.4,2.5"/>
 
       <param name="use_ptp_timestamp"           value="true"/>
       <param name="ptp_enable_feature_name"     value="PtpEnable"/>

diff --git a/launch/respawning_camera_aravis.launch b/launch/respawning_camera_aravis.launch
@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+  <node pkg="camera_aravis" type="launch_script.sh" name="respawning_camera_aravis"
+        respawn="true" output="screen"
+        args="$(find camera_aravis)/launch/camera_aravis.launch"/>
+</launch> 
diff --git a/scripts/launch_script.sh 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
@@ -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<std::string>("ptp_set_cmd_feature_name", ptp_set_cmd_feature_);
 
   bool init_params_from_reconfig = pnh.param<bool>("init_params_from_dyn_reconfigure", true);
+
+  std::string awb_mode = pnh.param<std::string>("BalanceWhiteAuto", "Continuous");
+  std::string wb_ratio_selector_feature = pnh.param<std::string>("wb_ratio_selector_feature", "");
+  std::string wb_ratio_feature = pnh.param<std::string>("wb_ratio_feature", "");
+
+  std::string wb_ratio_selector_args = pnh.param<std::string>("wb_ratio_selectors", "");  
+  std::vector<std::string> wb_ratio_selector_values;
+  parseStringArgs(wb_ratio_selector_args, wb_ratio_selector_values);
+
+  std::string wb_ratio_args = pnh.param<std::string>("wb_ratios", "");  
+  std::vector<std::string> 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<bool>("ExtendedCameraInfo", pub_ext_camera_info_); // publish an extended camera info message
   pub_tf_optical_ = pnh.param<bool>("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<double>("diagnostic_publish_rate", 0.1));
 
+  shutdown_trigger_ = nh.createTimer(ros::Duration(0.1), &CameraAravisNodelet::onShutdownTriggered, 
+    this, true, false);
+  shutdown_delay_s_ = pnh.param<double>("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