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