camera_aravis_nodelet.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * camera_aravis
4  *
5  * Copyright © 2022 Fraunhofer IOSB and contributors
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Library General Public
9  * License as published by the Free Software Foundation; either
10  * version 2 of the License, or (at your option) any later version.
11  *
12  * This library is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15  * Library General Public License for more details.
16  *
17  * You should have received a copy of the GNU Library General Public
18  * License along with this library; if not, write to the
19  * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
20  * Boston, MA 02110-1301, USA.
21  *
22  ****************************************************************************/
23 
24 #include <camera_aravis/camera_aravis_nodelet.h>
25 
26 #include <thread>
27 
28 #include <pluginlib/class_list_macros.h>
29 PLUGINLIB_EXPORT_CLASS(camera_aravis::CameraAravisNodelet, nodelet::Nodelet)
30 
31 namespace camera_aravis
32 {
33 
34 CameraAravisNodelet::CameraAravisNodelet()
35 {
36 }
37 
38 CameraAravisNodelet::~CameraAravisNodelet()
39 {
40  for (int i = 0; i < p_streams_.size(); i++)
41  {
42  if (p_streams_[i])
43  {
44  arv_stream_set_emit_signals(p_streams_[i], FALSE);
45  }
46  }
47 
48  spawning_ = false;
49  if (spawn_stream_thread_.joinable())
50  {
51  spawn_stream_thread_.join();
52  }
53 
54  software_trigger_active_ = false;
55  if (software_trigger_thread_.joinable())
56  {
57  software_trigger_thread_.join();
58  }
59 
60  tf_thread_active_ = false;
61  if (tf_dyn_thread_.joinable())
62  {
63  tf_dyn_thread_.join();
64  }
65 
66  diagnostic_thread_active_ = false;
67  if (diagnostic_thread_.joinable())
68  {
69  diagnostic_thread_.join();
70  }
71 
72  for (int i = 0; i < p_streams_.size(); i++)
73  {
74  guint64 n_completed_buffers = 0;
75  guint64 n_failures = 0;
76  guint64 n_underruns = 0;
77  arv_stream_get_statistics(p_streams_[i], &n_completed_buffers, &n_failures, &n_underruns);
78  ROS_INFO("Completed buffers = %Lu", (unsigned long long)n_completed_buffers);
79  ROS_INFO("Failures = %Lu", (unsigned long long)n_failures);
80  ROS_INFO("Underruns = %Lu", (unsigned long long)n_underruns);
81  if (arv_camera_is_gv_device(p_camera_))
82  {
83  guint64 n_resent;
84  guint64 n_missing;
85  arv_gv_stream_get_statistics(reinterpret_cast<ArvGvStream*>(p_streams_[i]), &n_resent, &n_missing);
86  ROS_INFO("Resent buffers = %Lu", (unsigned long long)n_resent);
87  ROS_INFO("Missing = %Lu", (unsigned long long)n_missing);
88  }
89  }
90 
91  if (p_device_)
92  {
93  arv_device_execute_command(p_device_, "AcquisitionStop");
94  }
95 
96  for (int i = 0; i < p_streams_.size(); i++)
97  {
98  g_object_unref(p_streams_[i]);
99  }
100  g_object_unref(p_camera_);
101 }
102 
103 void CameraAravisNodelet::onInit()
104 {
105  ros::NodeHandle nh = getNodeHandle();
106  ros::NodeHandle pnh = getPrivateNodeHandle();
107 
108  // Retrieve ros parameters
109  verbose_ = pnh.param<bool>("verbose", verbose_);
110  guid_ = pnh.param<std::string>("guid", guid_); // Get the camera guid as a parameter or use the first device.
111 
112  use_ptp_stamp_ = pnh.param<bool>("use_ptp_timestamp", use_ptp_stamp_);
113  ptp_enable_feature_ = pnh.param<std::string>("ptp_enable_feature_name", ptp_enable_feature_);
114  ptp_status_feature_ = pnh.param<std::string>("ptp_status_feature_name", ptp_status_feature_);
115  ptp_set_cmd_feature_ = pnh.param<std::string>("ptp_set_cmd_feature_name", ptp_set_cmd_feature_);
116 
117  bool init_params_from_reconfig = pnh.param<bool>("init_params_from_dyn_reconfigure", true);
118 
119  std::string awb_mode = pnh.param<std::string>("BalanceWhiteAuto", "Continuous");
120  std::string wb_ratio_selector_feature = pnh.param<std::string>("wb_ratio_selector_feature", "");
121  std::string wb_ratio_feature = pnh.param<std::string>("wb_ratio_feature", "");
122 
123  std::string wb_ratio_selector_args = pnh.param<std::string>("wb_ratio_selectors", "");
124  std::vector<std::string> wb_ratio_selector_values;
125  parseStringArgs(wb_ratio_selector_args, wb_ratio_selector_values);
126 
127  std::string wb_ratio_args = pnh.param<std::string>("wb_ratios", "");
128  std::vector<std::string> wb_ratio_str_values;
129  parseStringArgs(wb_ratio_args, wb_ratio_str_values);
130 
131  if (wb_ratio_selector_values.size() != wb_ratio_str_values.size())
132  {
133  ROS_WARN("Lists of 'wb_ratio_selector' and 'wb_ratio' have different sizes. "
134  "Only setting values which exist in both lists.");
135  }
136 
137  pub_ext_camera_info_ = pnh.param<bool>("ExtendedCameraInfo", pub_ext_camera_info_); // publish an extended camera info message
138  pub_tf_optical_ = pnh.param<bool>("publish_tf", pub_tf_optical_); // should we publish tf transforms to camera optical frame?
139 
140  std::string stream_channel_args;
141  if (pnh.getParam("channel_names", stream_channel_args))
142  {
143  parseStringArgs(stream_channel_args, stream_names_);
144  }
145  else
146  {
147  stream_names_ = {""};
148  }
149 
150  std::string pixel_format_args;
151  std::vector<std::string> pixel_formats;
152  pnh.param("PixelFormat", pixel_format_args, pixel_format_args);
153  parseStringArgs(pixel_format_args, pixel_formats);
154 
155  std::string calib_url_args;
156  std::vector<std::string> calib_urls;
157  pnh.param("camera_info_url", calib_url_args, calib_url_args);
158  parseStringArgs(calib_url_args, calib_urls);
159 
160  diagnostic_yaml_url_ = pnh.param<std::string>("diagnostic_yaml_url", diagnostic_yaml_url_);
161  diagnostic_publish_rate_ =
162  std::max(0.0, pnh.param<double>("diagnostic_publish_rate", 0.1));
163 
164  shutdown_trigger_ = nh.createTimer(ros::Duration(0.1), &CameraAravisNodelet::onShutdownTriggered,
165  this, true, false);
166  shutdown_delay_s_ = pnh.param<double>("shutdown_delay_s", 5.0);
167 
168  // Print out some useful info.
169  ROS_INFO("Attached cameras:");
170  arv_update_device_list();
171  uint n_interfaces = arv_get_n_interfaces();
172  ROS_INFO("# Interfaces: %d", n_interfaces);
173 
174  uint n_devices = arv_get_n_devices();
175  ROS_INFO("# Devices: %d", n_devices);
176  for (uint i = 0; i < n_devices; i++)
177  ROS_INFO("Device%d: %s", i, arv_get_device_id(i));
178 
179  if (n_devices == 0)
180  {
181  ROS_ERROR("No cameras detected.");
182  return;
183  }
184 
185  // Open the camera, and set it up.
186  while (!p_camera_)
187  {
188  if (guid_.empty())
189  {
190  ROS_INFO("Opening: (any)");
191  p_camera_ = arv_camera_new(NULL);
192  }
193  else
194  {
195  ROS_INFO_STREAM("Opening: " << guid_);
196  p_camera_ = arv_camera_new(guid_.c_str());
197  }
198  ros::Duration(1.0).sleep();
199  }
200 
201  p_device_ = arv_camera_get_device(p_camera_);
202  const char* vendor_name = arv_camera_get_vendor_name(p_camera_);
203  const char* model_name = arv_camera_get_model_name(p_camera_);
204  const char* device_id = arv_camera_get_device_id(p_camera_);
205  const char* device_sn = arv_device_get_string_feature_value(p_device_, "DeviceSerialNumber");
206  ROS_INFO("Successfully Opened: %s-%s-%s", vendor_name, model_name,
207  (device_sn) ? device_sn : device_id);
208 
209  // Start the dynamic_reconfigure server.
210  reconfigure_server_.reset(new dynamic_reconfigure::Server<Config>(reconfigure_mutex_, pnh));
211  reconfigure_server_->getConfigDefault(config_);
212  reconfigure_server_->getConfigMin(config_min_);
213  reconfigure_server_->getConfigMax(config_max_);
214 
215  // See which features exist in this camera device
216  discoverFeatures();
217 
218  // Check the number of streams for this camera
219  num_streams_ = arv_device_get_integer_feature_value(p_device_, "DeviceStreamChannelCount");
220  // if this return 0, try the deprecated GevStreamChannelCount in case this is an older camera
221  if (!num_streams_)
222  {
223  num_streams_ = arv_device_get_integer_feature_value(p_device_, "GevStreamChannelCount");
224  }
225  // if this also returns 0, assume number of streams = 1
226  if (!num_streams_)
227  {
228  ROS_WARN("Unable to detect number of supported stream channels.");
229  num_streams_ = 1;
230  }
231 
232  ROS_INFO("Number of supported stream channels %i.", (int)num_streams_);
233 
234  // check if every stream channel has been given a channel name
235  if (stream_names_.size() < num_streams_)
236  {
237  num_streams_ = stream_names_.size();
238  }
239 
240  // initialize the sensor structs
241  for (int i = 0; i < num_streams_; i++)
242  {
243  sensors_.push_back(new Sensor());
244  p_streams_.push_back(NULL);
245  p_buffer_pools_.push_back(CameraBufferPool::Ptr());
246  p_camera_info_managers_.push_back(NULL);
247  p_camera_info_node_handles_.push_back(NULL);
248  camera_infos_.push_back(sensor_msgs::CameraInfoPtr());
249  cam_pubs_.push_back(image_transport::CameraPublisher());
250  extended_camera_info_pubs_.push_back(ros::Publisher());
251  }
252 
253  // Get parameter bounds.
254  arv_camera_get_exposure_time_bounds(p_camera_, &config_min_.ExposureTime, &config_max_.ExposureTime);
255  arv_camera_get_gain_bounds(p_camera_, &config_min_.Gain, &config_max_.Gain);
256  for (int i = 0; i < num_streams_; i++)
257  {
258  arv_camera_gv_select_stream_channel(p_camera_, i);
259  arv_camera_get_sensor_size(p_camera_, &sensors_[i]->width, &sensors_[i]->height);
260  }
261  arv_camera_get_width_bounds(p_camera_, &roi_.width_min, &roi_.width_max);
262  arv_camera_get_height_bounds(p_camera_, &roi_.height_min, &roi_.height_max);
263  arv_camera_get_frame_rate_bounds(p_camera_, &config_min_.AcquisitionFrameRate, &config_max_.AcquisitionFrameRate);
264  if (implemented_features_["FocusPos"])
265  {
266  gint64 focus_min64, focus_max64;
267  arv_device_get_integer_feature_bounds(p_device_, "FocusPos", &focus_min64, &focus_max64);
268  config_min_.FocusPos = focus_min64;
269  config_max_.FocusPos = focus_max64;
270  }
271  else
272  {
273  config_min_.FocusPos = 0;
274  config_max_.FocusPos = 0;
275  }
276 
277  for (int i = 0; i < num_streams_; i++)
278  {
279  arv_camera_gv_select_stream_channel(p_camera_, i);
280 
281  if (init_params_from_reconfig)
282  {
283  // Initial camera settings.
284  if (implemented_features_["ExposureTime"])
285  arv_camera_set_exposure_time(p_camera_, config_.ExposureTime);
286  else if (implemented_features_["ExposureTimeAbs"])
287  arv_device_set_float_feature_value(p_device_, "ExposureTimeAbs", config_.ExposureTime);
288  if (implemented_features_["Gain"])
289  arv_camera_set_gain(p_camera_, config_.Gain);
290  if (implemented_features_["AcquisitionFrameRateEnable"])
291  arv_device_set_integer_feature_value(p_device_, "AcquisitionFrameRateEnable", 1);
292  if (implemented_features_["AcquisitionFrameRate"])
293  arv_camera_set_frame_rate(p_camera_, config_.AcquisitionFrameRate);
294 
295  // init default to full sensor resolution
296  arv_camera_set_region(p_camera_, 0, 0, roi_.width_max, roi_.height_max);
297 
298  // Set up the triggering.
299  if (implemented_features_["TriggerMode"] && implemented_features_["TriggerSelector"])
300  {
301  arv_device_set_string_feature_value(p_device_, "TriggerSelector", "FrameStart");
302  arv_device_set_string_feature_value(p_device_, "TriggerMode", "Off");
303  }
304  }
305 
306  // possibly set or override from given parameter
307  writeCameraFeaturesFromRosparam();
308 
309  if (awb_mode == "Off" && !wb_ratio_selector_feature.empty() && implemented_features_[wb_ratio_selector_feature] && !wb_ratio_feature.empty() && implemented_features_[wb_ratio_feature])
310  {
311  for (int l = 0; l < std::min(wb_ratio_selector_values.size(), wb_ratio_str_values.size());
312  l++)
313  {
314  arv_device_set_string_feature_value(p_device_, wb_ratio_selector_feature.c_str(),
315  wb_ratio_selector_values[l].c_str());
316  arv_device_set_float_feature_value(p_device_, wb_ratio_feature.c_str(),
317  std::stof(wb_ratio_str_values[l]));
318  }
319  }
320  }
321 
322  ros::Duration(2.0).sleep();
323 
324  // get current state of camera for config_
325  arv_camera_get_region(p_camera_, &roi_.x, &roi_.y, &roi_.width, &roi_.height);
326  config_.AcquisitionMode =
327  implemented_features_["AcquisitionMode"] ? arv_device_get_string_feature_value(p_device_, "AcquisitionMode") : "Continuous";
328  config_.AcquisitionFrameRate =
329  implemented_features_["AcquisitionFrameRate"] ? arv_camera_get_frame_rate(p_camera_) : 0.0;
330  config_.ExposureAuto =
331  implemented_features_["ExposureAuto"] ? arv_device_get_string_feature_value(p_device_, "ExposureAuto") : "Off";
332  config_.ExposureTime = implemented_features_["ExposureTime"] ? arv_camera_get_exposure_time(p_camera_) : 0.0;
333  config_.GainAuto =
334  implemented_features_["GainAuto"] ? arv_device_get_string_feature_value(p_device_, "GainAuto") : "Off";
335  config_.Gain = implemented_features_["Gain"] ? arv_camera_get_gain(p_camera_) : 0.0;
336  config_.TriggerMode =
337  implemented_features_["TriggerMode"] ? arv_device_get_string_feature_value(p_device_, "TriggerMode") : "Off";
338  config_.TriggerSource =
339  implemented_features_["TriggerSource"] ? arv_device_get_string_feature_value(p_device_, "TriggerSource") : "Software";
340 
341  // get pixel format name and translate into corresponding ROS name
342  for (int i = 0; i < num_streams_; i++)
343  {
344  arv_camera_gv_select_stream_channel(p_camera_, i);
345  std::string source_selector = "Source" + std::to_string(i);
346  arv_device_set_string_feature_value(p_device_, "SourceSelector", source_selector.c_str());
347  arv_device_set_string_feature_value(p_device_, "PixelFormat", pixel_formats[i].c_str());
348  sensors_[i]->pixel_format = std::string(arv_device_get_string_feature_value(p_device_, "PixelFormat"));
349  const auto sensor_iter = CONVERSIONS_DICTIONARY.find(sensors_[i]->pixel_format);
350  if (sensor_iter != CONVERSIONS_DICTIONARY.end())
351  {
352  convert_formats.push_back(sensor_iter->second);
353  }
354  else
355  {
356  ROS_WARN_STREAM("There is no known conversion from " << sensors_[i]->pixel_format << " to a usual ROS image encoding. Likely you need to implement one.");
357  }
358 
359  sensors_[i]->n_bits_pixel = ARV_PIXEL_FORMAT_BIT_PER_PIXEL(
360  arv_device_get_integer_feature_value(p_device_, "PixelFormat"));
361  config_.FocusPos =
362  implemented_features_["FocusPos"] ? arv_device_get_integer_feature_value(p_device_, "FocusPos") : 0;
363  }
364 
365  // Get other (non GenIcam) parameter current values.
366  pnh.param<double>("softwaretriggerrate", config_.softwaretriggerrate, config_.softwaretriggerrate);
367  pnh.param<int>("mtu", config_.mtu, config_.mtu);
368  pnh.param<std::string>("frame_id", config_.frame_id, config_.frame_id);
369  pnh.param<bool>("auto_master", config_.AutoMaster, config_.AutoMaster);
370  pnh.param<bool>("auto_slave", config_.AutoSlave, config_.AutoSlave);
371  setAutoMaster(config_.AutoMaster);
372  setAutoSlave(config_.AutoSlave);
373 
374  if (pub_tf_optical_)
375  {
376  tf_optical_.header.frame_id = config_.frame_id;
377  tf_optical_.child_frame_id = config_.frame_id + "_optical";
378  tf_optical_.transform.translation.x = 0.0;
379  tf_optical_.transform.translation.y = 0.0;
380  tf_optical_.transform.translation.z = 0.0;
381  tf_optical_.transform.rotation.x = -0.5;
382  tf_optical_.transform.rotation.y = 0.5;
383  tf_optical_.transform.rotation.z = -0.5;
384  tf_optical_.transform.rotation.w = 0.5;
385 
386  double tf_publish_rate;
387  pnh.param<double>("tf_publish_rate", tf_publish_rate, 0);
388  if (tf_publish_rate > 0.)
389  {
390  // publish dynamic tf at given rate (recommended when running as a Nodelet, since latching has bugs-by-design)
391  p_tb_.reset(new tf2_ros::TransformBroadcaster());
392  tf_dyn_thread_ = std::thread(&CameraAravisNodelet::publishTfLoop, this, tf_publish_rate);
393  }
394  else
395  {
396  // publish static tf only once (latched)
397  p_stb_.reset(new tf2_ros::StaticTransformBroadcaster());
398  tf_optical_.header.stamp = ros::Time::now();
399  p_stb_->sendTransform(tf_optical_);
400  }
401  }
402 
403  // read diagnostic features from yaml file and initialize publishing thread
404  if (!diagnostic_yaml_url_.empty() && diagnostic_publish_rate_ > 0.0)
405  {
406  try
407  {
408  diagnostic_features_ = YAML::LoadFile(diagnostic_yaml_url_);
409  }
410  catch (const YAML::BadFile& e)
411  {
412  ROS_WARN("YAML file cannot be loaded: %s", e.what());
413  ROS_WARN("Camera diagnostics will not be published.");
414  }
415  catch (const YAML::ParserException& e)
416  {
417  ROS_WARN("YAML file is malformed: %s", e.what());
418  ROS_WARN("Camera diagnostics will not be published.");
419  }
420 
421  if (diagnostic_features_.size() > 0)
422  {
423  diagnostic_pub_ = getNodeHandle().advertise<CameraDiagnostics>(
424  ros::names::remap(this->getName() + "/diagnostics"), 1, true);
425 
426  diagnostic_thread_active_ = true;
427  diagnostic_thread_ = std::thread(&CameraAravisNodelet::readAndPublishCameraDiagnostics, this,
428  diagnostic_publish_rate_);
429  }
430  else
431  {
432  ROS_WARN("No diagnostic features specified.");
433  ROS_WARN("Camera diagnostics will not be published.");
434  }
435  }
436  else if (!diagnostic_yaml_url_.empty() && diagnostic_publish_rate_ <= 0.0)
437  {
438  ROS_WARN("diagnostic_publish_rate is <= 0.0");
439  ROS_WARN("Camera diagnostics will not be published.");
440  }
441 
442  // default calibration url is [DeviceSerialNumber/DeviceID].yaml
443  ArvGcNode* p_gc_node;
444  GError* error = NULL;
445 
446  p_gc_node = arv_device_get_feature(p_device_, "DeviceSerialNumber");
447 
448  if (calib_urls[0].empty())
449  {
450  if (arv_gc_feature_node_is_implemented(ARV_GC_FEATURE_NODE(p_gc_node), &error))
451  {
452  GType device_serial_return_type = arv_gc_feature_node_get_value_type(ARV_GC_FEATURE_NODE(p_gc_node));
453  // If the feature DeviceSerialNumber is not string, it indicates that the camera is using an older version of the genicam SFNC.
454  // Older camera models do not have a DeviceSerialNumber as string, but as integer and often set to 0.
455  // In those cases use the outdated DeviceID (deprecated since genicam SFNC v2.0).
456  if (device_serial_return_type == G_TYPE_STRING)
457  {
458  calib_urls[0] = arv_device_get_string_feature_value(p_device_, "DeviceSerialNumber");
459  calib_urls[0] += ".yaml";
460  }
461  else if (device_serial_return_type == G_TYPE_INT64)
462  {
463  calib_urls[0] = arv_device_get_string_feature_value(p_device_, "DeviceID");
464  calib_urls[0] += ".yaml";
465  }
466  }
467  }
468 
469  for (int i = 0; i < num_streams_; i++)
470  {
471  // Start the camerainfo manager.
472  std::string camera_info_frame_id = config_.frame_id;
473  if (!stream_names_[i].empty())
474  camera_info_frame_id = config_.frame_id + '/' + stream_names_[i];
475 
476  // Use separate node handles for CameraInfoManagers when using a Multisource Camera
477  if (!stream_names_[i].empty())
478  {
479  p_camera_info_node_handles_[i].reset(new ros::NodeHandle(pnh, stream_names_[i]));
480  p_camera_info_managers_[i].reset(new camera_info_manager::CameraInfoManager(*p_camera_info_node_handles_[i], camera_info_frame_id, calib_urls[i]));
481  }
482  else
483  {
484  p_camera_info_managers_[i].reset(new camera_info_manager::CameraInfoManager(pnh, camera_info_frame_id, calib_urls[i]));
485  }
486 
487  ROS_INFO("Reset %s Camera Info Manager", stream_names_[i].c_str());
488  ROS_INFO("%s Calib URL: %s", stream_names_[i].c_str(), calib_urls[i].c_str());
489 
490  // publish an ExtendedCameraInfo message
491  setExtendedCameraInfo(stream_names_[i], i);
492  }
493 
494  // update the reconfigure config
495  reconfigure_server_->setConfigMin(config_min_);
496  reconfigure_server_->setConfigMax(config_max_);
497  reconfigure_server_->updateConfig(config_);
498  ros::Duration(2.0).sleep();
499  reconfigure_server_->setCallback(boost::bind(&CameraAravisNodelet::rosReconfigureCallback, this, _1, _2));
500 
501  // Print information.
502  ROS_INFO(" Using Camera Configuration:");
503  ROS_INFO(" ---------------------------");
504  ROS_INFO(" Vendor name = %s", arv_device_get_string_feature_value(p_device_, "DeviceVendorName"));
505  ROS_INFO(" Model name = %s", arv_device_get_string_feature_value(p_device_, "DeviceModelName"));
506  ROS_INFO(" Device id = %s", arv_device_get_string_feature_value(p_device_, "DeviceUserID"));
507  ROS_INFO(" Serial number = %s", arv_device_get_string_feature_value(p_device_, "DeviceSerialNumber"));
508  ROS_INFO(
509  " Type = %s",
510  arv_camera_is_uv_device(p_camera_) ? "USB3Vision" : (arv_camera_is_gv_device(p_camera_) ? "GigEVision" : "Other"));
511  ROS_INFO(" Sensor width = %d", sensors_[0]->width);
512  ROS_INFO(" Sensor height = %d", sensors_[0]->height);
513  ROS_INFO(" ROI x,y,w,h = %d, %d, %d, %d", roi_.x, roi_.y, roi_.width, roi_.height);
514  ROS_INFO(" Pixel format = %s", sensors_[0]->pixel_format.c_str());
515  ROS_INFO(" BitsPerPixel = %lu", sensors_[0]->n_bits_pixel);
516  ROS_INFO(" Acquisition Mode = %s",
517  implemented_features_["AcquisitionMode"] ? arv_device_get_string_feature_value(p_device_, "AcquisitionMode") : "(not implemented in camera)");
518  ROS_INFO(" Trigger Mode = %s",
519  implemented_features_["TriggerMode"] ? arv_device_get_string_feature_value(p_device_, "TriggerMode") : "(not implemented in camera)");
520  ROS_INFO(" Trigger Source = %s",
521  implemented_features_["TriggerSource"] ? arv_device_get_string_feature_value(p_device_, "TriggerSource") : "(not implemented in camera)");
522  ROS_INFO(" Can set FrameRate: %s", implemented_features_["AcquisitionFrameRate"] ? "True" : "False");
523  if (implemented_features_["AcquisitionFrameRate"])
524  {
525  ROS_INFO(" AcquisitionFrameRate = %g hz", config_.AcquisitionFrameRate);
526  }
527 
528  if (implemented_features_["BalanceWhiteAuto"])
529  {
530  ROS_INFO(" BalanceWhiteAuto = %s", awb_mode.c_str());
531  if (awb_mode != "Continuous" && !wb_ratio_selector_feature.empty() && implemented_features_[wb_ratio_selector_feature] && !wb_ratio_feature.empty() && implemented_features_[wb_ratio_feature])
532  {
533  for (int l = 0; l < std::min(wb_ratio_selector_values.size(), wb_ratio_str_values.size());
534  l++)
535  {
536  arv_device_set_string_feature_value(p_device_, wb_ratio_selector_feature.c_str(),
537  wb_ratio_selector_values[l].c_str());
538  float wb_ratio_val = arv_device_get_float_feature_value(p_device_, wb_ratio_feature.c_str());
539  ROS_INFO(" BalanceRatio %s = %f", wb_ratio_selector_values[l].c_str(), wb_ratio_val);
540  }
541  }
542  }
543 
544  ROS_INFO(" Can set Exposure: %s", implemented_features_["ExposureTime"] ? "True" : "False");
545  if (implemented_features_["ExposureTime"])
546  {
547  ROS_INFO(" Can set ExposureAuto: %s", implemented_features_["ExposureAuto"] ? "True" : "False");
548  ROS_INFO(" Exposure = %g us in range [%g,%g]", config_.ExposureTime, config_min_.ExposureTime,
549  config_max_.ExposureTime);
550  }
551 
552  ROS_INFO(" Can set Gain: %s", implemented_features_["Gain"] ? "True" : "False");
553  if (implemented_features_["Gain"])
554  {
555  ROS_INFO(" Can set GainAuto: %s", implemented_features_["GainAuto"] ? "True" : "False");
556  ROS_INFO(" Gain = %f %% in range [%f,%f]", config_.Gain, config_min_.Gain, config_max_.Gain);
557  }
558 
559  ROS_INFO(" Can set FocusPos: %s", implemented_features_["FocusPos"] ? "True" : "False");
560 
561  if (implemented_features_["GevSCPSPacketSize"])
562  ROS_INFO(" Network mtu = %lu", arv_device_get_integer_feature_value(p_device_, "GevSCPSPacketSize"));
563 
564  ROS_INFO(" ---------------------------");
565 
566  // Reset PTP clock
567  if (use_ptp_stamp_)
568  {
569  resetPtpClock();
570 
571  if (!ptp_set_cmd_feature_.empty())
572  {
573  if (implemented_features_[ptp_set_cmd_feature_])
574  {
575  arv_device_execute_command(p_device_, ptp_set_cmd_feature_.c_str());
576  }
577  else
578  ROS_WARN("PTP Error: ptp_set_cmd_feature_ '%s' is not available on the device.",
579  ptp_set_cmd_feature_.c_str());
580  }
581  }
582 
583  // spawn camera stream in thread, so onInit() is not blocked
584  spawning_ = true;
585  spawn_stream_thread_ = std::thread(&CameraAravisNodelet::spawnStream, this);
586 }
587 
588 void CameraAravisNodelet::spawnStream()
589 {
590  ros::NodeHandle nh = getNodeHandle();
591  ros::NodeHandle pnh = getPrivateNodeHandle();
592 
593  for (int i = 0; i < num_streams_; i++)
594  {
595  while (spawning_)
596  {
597  arv_camera_gv_select_stream_channel(p_camera_, i);
598  p_streams_[i] = arv_camera_create_stream(p_camera_, NULL, NULL);
599 
600  if (p_streams_[i])
601  {
602  // Load up some buffers.
603  arv_camera_gv_select_stream_channel(p_camera_, i);
604  const gint n_bytes_payload_stream_ = arv_camera_get_payload(p_camera_);
605 
606  p_buffer_pools_[i].reset(new CameraBufferPool(p_streams_[i], n_bytes_payload_stream_, 10));
607 
608  if (arv_camera_is_gv_device(p_camera_))
609  {
610  tuneGvStream(reinterpret_cast<ArvGvStream*>(p_streams_[i]));
611  }
612  break;
613  }
614  else
615  {
616  ROS_WARN("Stream %i: Could not create image stream for %s. Retrying...", i, guid_.c_str());
617  ros::Duration(1.0).sleep();
618  ros::spinOnce();
619  }
620  }
621  }
622 
623  // Monitor whether anyone is subscribed to the camera stream
624  std::vector<image_transport::SubscriberStatusCallback> image_cbs_;
625  std::vector<ros::SubscriberStatusCallback> info_cbs_;
626 
627  image_transport::SubscriberStatusCallback image_cb = [this](const image_transport::SingleSubscriberPublisher& ssp)
628  { this->rosConnectCallback(); };
629  ros::SubscriberStatusCallback info_cb = [this](const ros::SingleSubscriberPublisher& ssp)
630  { this->rosConnectCallback(); };
631 
632  for (int i = 0; i < num_streams_; i++)
633  {
634  image_transport::ImageTransport* p_transport;
635  // Set up image_raw
636  std::string topic_name = this->getName();
637  p_transport = new image_transport::ImageTransport(pnh);
638  if (num_streams_ != 1 || !stream_names_[i].empty())
639  {
640  topic_name += "/" + stream_names_[i];
641  }
642 
643  cam_pubs_[i] = p_transport->advertiseCamera(
644  ros::names::remap(topic_name + "/image_raw"),
645  1, image_cb, image_cb, info_cb, info_cb);
646  }
647 
648  // Connect signals with callbacks.
649  for (int i = 0; i < num_streams_; i++)
650  {
651  StreamIdData* data = new StreamIdData();
652  data->can = this;
653  data->stream_id = i;
654  g_signal_connect(p_streams_[i], "new-buffer", (GCallback)CameraAravisNodelet::newBufferReadyCallback, data);
655  }
656  g_signal_connect(p_device_, "control-lost", (GCallback)CameraAravisNodelet::controlLostCallback, this);
657 
658  for (int i = 0; i < num_streams_; i++)
659  {
660  arv_stream_set_emit_signals(p_streams_[i], TRUE);
661  }
662 
663  if (std::any_of(cam_pubs_.begin(), cam_pubs_.end(),
664  [](image_transport::CameraPublisher pub)
665  { return pub.getNumSubscribers() > 0; }))
666  {
667  arv_camera_start_acquisition(p_camera_);
668  }
669 
670  this->get_integer_service_ = pnh.advertiseService("get_integer_feature_value", &CameraAravisNodelet::getIntegerFeatureCallback, this);
671  this->get_float_service_ = pnh.advertiseService("get_float_feature_value", &CameraAravisNodelet::getFloatFeatureCallback, this);
672  this->get_string_service_ = pnh.advertiseService("get_string_feature_value", &CameraAravisNodelet::getStringFeatureCallback, this);
673  this->get_boolean_service_ = pnh.advertiseService("get_boolean_feature_value", &CameraAravisNodelet::getBooleanFeatureCallback, this);
674 
675  this->set_integer_service_ = pnh.advertiseService("set_integer_feature_value", &CameraAravisNodelet::setIntegerFeatureCallback, this);
676  this->set_float_service_ = pnh.advertiseService("set_float_feature_value", &CameraAravisNodelet::setFloatFeatureCallback, this);
677  this->set_string_service_ = pnh.advertiseService("set_string_feature_value", &CameraAravisNodelet::setStringFeatureCallback, this);
678  this->set_boolean_service_ = pnh.advertiseService("set_boolean_feature_value", &CameraAravisNodelet::setBooleanFeatureCallback, this);
679 
680  ROS_INFO("Done initializing camera_aravis.");
681 }
682 
683 bool CameraAravisNodelet::getIntegerFeatureCallback(camera_aravis::get_integer_feature_value::Request& request, camera_aravis::get_integer_feature_value::Response& response)
684 {
685  const char* feature_name = request.feature.c_str();
686  response.response = arv_device_get_integer_feature_value(this->p_device_, feature_name);
687  return true;
688 }
689 
690 bool CameraAravisNodelet::setIntegerFeatureCallback(camera_aravis::set_integer_feature_value::Request& request, camera_aravis::set_integer_feature_value::Response& response)
691 {
692  const char* feature_name = request.feature.c_str();
693  guint64 value = request.value;
694  arv_device_set_integer_feature_value(this->p_device_, feature_name, value);
695  if (arv_device_get_status(this->p_device_) == ARV_DEVICE_STATUS_SUCCESS)
696  {
697  response.ok = true;
698  }
699  else
700  {
701  response.ok = false;
702  }
703  return true;
704 }
705 
706 bool CameraAravisNodelet::getFloatFeatureCallback(camera_aravis::get_float_feature_value::Request& request, camera_aravis::get_float_feature_value::Response& response)
707 {
708  const char* feature_name = request.feature.c_str();
709  response.response = arv_device_get_float_feature_value(this->p_device_, feature_name);
710  return true;
711 }
712 
713 bool CameraAravisNodelet::setFloatFeatureCallback(camera_aravis::set_float_feature_value::Request& request, camera_aravis::set_float_feature_value::Response& response)
714 {
715  const char* feature_name = request.feature.c_str();
716  const double value = request.value;
717  arv_device_set_float_feature_value(this->p_device_, feature_name, value);
718  if (arv_device_get_status(this->p_device_) == ARV_DEVICE_STATUS_SUCCESS)
719  {
720  response.ok = true;
721  }
722  else
723  {
724  response.ok = false;
725  }
726  return true;
727 }
728 
729 bool CameraAravisNodelet::getStringFeatureCallback(camera_aravis::get_string_feature_value::Request& request, camera_aravis::get_string_feature_value::Response& response)
730 {
731  const char* feature_name = request.feature.c_str();
732  response.response = arv_device_get_string_feature_value(this->p_device_, feature_name);
733  return true;
734 }
735 
736 bool CameraAravisNodelet::setStringFeatureCallback(camera_aravis::set_string_feature_value::Request& request, camera_aravis::set_string_feature_value::Response& response)
737 {
738  const char* feature_name = request.feature.c_str();
739  const char* value = request.value.c_str();
740  arv_device_set_string_feature_value(this->p_device_, feature_name, value);
741  if (arv_device_get_status(this->p_device_) == ARV_DEVICE_STATUS_SUCCESS)
742  {
743  response.ok = true;
744  }
745  else
746  {
747  response.ok = false;
748  }
749  return true;
750 }
751 
752 bool CameraAravisNodelet::getBooleanFeatureCallback(camera_aravis::get_boolean_feature_value::Request& request, camera_aravis::get_boolean_feature_value::Response& response)
753 {
754  const char* feature_name = request.feature.c_str();
755  response.response = arv_device_get_boolean_feature_value(this->p_device_, feature_name);
756  return true;
757 }
758 
759 bool CameraAravisNodelet::setBooleanFeatureCallback(camera_aravis::set_boolean_feature_value::Request& request, camera_aravis::set_boolean_feature_value::Response& response)
760 {
761  const char* feature_name = request.feature.c_str();
762  const bool value = request.value;
763  arv_device_set_boolean_feature_value(this->p_device_, feature_name, value);
764  if (arv_device_get_status(this->p_device_) == ARV_DEVICE_STATUS_SUCCESS)
765  {
766  response.ok = true;
767  }
768  else
769  {
770  response.ok = false;
771  }
772  return true;
773 }
774 
775 void CameraAravisNodelet::resetPtpClock()
776 {
777  if (ptp_status_feature_.empty() || ptp_enable_feature_.empty())
778  {
779  ROS_WARN("PTP Error: ptp_status_feature_name and/or ptp_enable_feature_name are empty.");
780  return;
781  }
782 
783  if (!implemented_features_[ptp_status_feature_] || !implemented_features_[ptp_enable_feature_])
784  {
785  if (!implemented_features_[ptp_status_feature_])
786  ROS_WARN("PTP Error: ptp_status_feature_name '%s' is not available on the device.",
787  ptp_status_feature_.c_str());
788  if (!implemented_features_[ptp_enable_feature_])
789  ROS_WARN("PTP Error: ptp_enable_feature_name '%s' is not available on the device.",
790  ptp_enable_feature_.c_str());
791  return;
792  }
793 
794  // a PTP slave can take the following states: Slave, Listening, Uncalibrated, Faulty, Disabled
795  const char* ptpS_satus_char_ptr =
796  arv_device_get_string_feature_value(p_device_, ptp_status_feature_.c_str());
797  std::string ptp_status_str = (ptpS_satus_char_ptr) ? std::string(ptpS_satus_char_ptr) : "Faulty";
798  bool ptp_is_enabled = arv_device_get_boolean_feature_value(p_device_, ptp_enable_feature_.c_str());
799  if (ptp_status_str == "Faulty" ||
800  ptp_status_str == "Disabled" ||
801  ptp_status_str == "Initializing" ||
802  !ptp_is_enabled)
803  {
804  ROS_INFO("Resetting ptp clock (was set to %s)", ptp_status_str.c_str());
805 
806  arv_device_set_boolean_feature_value(p_device_, ptp_enable_feature_.c_str(), false);
807  arv_device_set_boolean_feature_value(p_device_, ptp_enable_feature_.c_str(), true);
808  }
809 }
810 
811 void CameraAravisNodelet::cameraAutoInfoCallback(const CameraAutoInfoConstPtr& msg_ptr)
812 {
813  if (config_.AutoSlave && p_device_)
814  {
815 
816  if (auto_params_.exposure_time != msg_ptr->exposure_time && implemented_features_["ExposureTime"])
817  {
818  arv_device_set_float_feature_value(p_device_, "ExposureTime", msg_ptr->exposure_time);
819  }
820 
821  if (implemented_features_["Gain"])
822  {
823  if (auto_params_.gain != msg_ptr->gain)
824  {
825  if (implemented_features_["GainSelector"])
826  {
827  arv_device_set_string_feature_value(p_device_, "GainSelector", "All");
828  }
829  arv_device_set_float_feature_value(p_device_, "Gain", msg_ptr->gain);
830  }
831 
832  if (implemented_features_["GainSelector"])
833  {
834  if (auto_params_.gain_red != msg_ptr->gain_red)
835  {
836  arv_device_set_string_feature_value(p_device_, "GainSelector", "Red");
837  arv_device_set_float_feature_value(p_device_, "Gain", msg_ptr->gain_red);
838  }
839 
840  if (auto_params_.gain_green != msg_ptr->gain_green)
841  {
842  arv_device_set_string_feature_value(p_device_, "GainSelector", "Green");
843  arv_device_set_float_feature_value(p_device_, "Gain", msg_ptr->gain_green);
844  }
845 
846  if (auto_params_.gain_blue != msg_ptr->gain_blue)
847  {
848  arv_device_set_string_feature_value(p_device_, "GainSelector", "Blue");
849  arv_device_set_float_feature_value(p_device_, "Gain", msg_ptr->gain_blue);
850  }
851  }
852  }
853 
854  if (implemented_features_["BlackLevel"])
855  {
856  if (auto_params_.black_level != msg_ptr->black_level)
857  {
858  if (implemented_features_["BlackLevelSelector"])
859  {
860  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "All");
861  }
862  arv_device_set_float_feature_value(p_device_, "BlackLevel", msg_ptr->black_level);
863  }
864 
865  if (implemented_features_["BlackLevelSelector"])
866  {
867  if (auto_params_.bl_red != msg_ptr->bl_red)
868  {
869  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "Red");
870  arv_device_set_float_feature_value(p_device_, "BlackLevel", msg_ptr->bl_red);
871  }
872 
873  if (auto_params_.bl_green != msg_ptr->bl_green)
874  {
875  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "Green");
876  arv_device_set_float_feature_value(p_device_, "BlackLevel", msg_ptr->bl_green);
877  }
878 
879  if (auto_params_.bl_blue != msg_ptr->bl_blue)
880  {
881  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "Blue");
882  arv_device_set_float_feature_value(p_device_, "BlackLevel", msg_ptr->bl_blue);
883  }
884  }
885  }
886 
887  // White balance as TIS is providing
888  if (strcmp("The Imaging Source Europe GmbH", arv_camera_get_vendor_name(p_camera_)) == 0)
889  {
890  arv_device_set_integer_feature_value(p_device_, "WhiteBalanceRedRegister", (int)(auto_params_.wb_red * 255.));
891  arv_device_set_integer_feature_value(p_device_, "WhiteBalanceGreenRegister", (int)(auto_params_.wb_green * 255.));
892  arv_device_set_integer_feature_value(p_device_, "WhiteBalanceBlueRegister", (int)(auto_params_.wb_blue * 255.));
893  }
894  else if (implemented_features_["BalanceRatio"] && implemented_features_["BalanceRatioSelector"])
895  {
896  if (auto_params_.wb_red != msg_ptr->wb_red)
897  {
898  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Red");
899  arv_device_set_float_feature_value(p_device_, "BalanceRatio", msg_ptr->wb_red);
900  }
901 
902  if (auto_params_.wb_green != msg_ptr->wb_green)
903  {
904  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Green");
905  arv_device_set_float_feature_value(p_device_, "BalanceRatio", msg_ptr->wb_green);
906  }
907 
908  if (auto_params_.wb_blue != msg_ptr->wb_blue)
909  {
910  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Blue");
911  arv_device_set_float_feature_value(p_device_, "BalanceRatio", msg_ptr->wb_blue);
912  }
913  }
914 
915  auto_params_ = *msg_ptr;
916  }
917 }
918 
919 void CameraAravisNodelet::syncAutoParameters()
920 {
921  auto_params_.exposure_time = auto_params_.gain = auto_params_.gain_red = auto_params_.gain_green =
922  auto_params_.gain_blue = auto_params_.black_level = auto_params_.bl_red = auto_params_.bl_green =
923  auto_params_.bl_blue = auto_params_.wb_red = auto_params_.wb_green = auto_params_.wb_blue =
924  std::numeric_limits<double>::quiet_NaN();
925 
926  if (p_device_)
927  {
928  if (implemented_features_["ExposureTime"])
929  {
930  auto_params_.exposure_time = arv_device_get_float_feature_value(p_device_, "ExposureTime");
931  }
932 
933  if (implemented_features_["Gain"])
934  {
935  if (implemented_features_["GainSelector"])
936  {
937  arv_device_set_string_feature_value(p_device_, "GainSelector", "All");
938  }
939  auto_params_.gain = arv_device_get_float_feature_value(p_device_, "Gain");
940  if (implemented_features_["GainSelector"])
941  {
942  arv_device_set_string_feature_value(p_device_, "GainSelector", "Red");
943  auto_params_.gain_red = arv_device_get_float_feature_value(p_device_, "Gain");
944  arv_device_set_string_feature_value(p_device_, "GainSelector", "Green");
945  auto_params_.gain_green = arv_device_get_float_feature_value(p_device_, "Gain");
946  arv_device_set_string_feature_value(p_device_, "GainSelector", "Blue");
947  auto_params_.gain_blue = arv_device_get_float_feature_value(p_device_, "Gain");
948  }
949  }
950 
951  if (implemented_features_["BlackLevel"])
952  {
953  if (implemented_features_["BlackLevelSelector"])
954  {
955  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "All");
956  }
957  auto_params_.black_level = arv_device_get_float_feature_value(p_device_, "BlackLevel");
958  if (implemented_features_["BlackLevelSelector"])
959  {
960  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "Red");
961  auto_params_.bl_red = arv_device_get_float_feature_value(p_device_, "BlackLevel");
962  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "Green");
963  auto_params_.bl_green = arv_device_get_float_feature_value(p_device_, "BlackLevel");
964  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "Blue");
965  auto_params_.bl_blue = arv_device_get_float_feature_value(p_device_, "BlackLevel");
966  }
967  }
968 
969  // White balance as TIS is providing
970  if (strcmp("The Imaging Source Europe GmbH", arv_camera_get_vendor_name(p_camera_)) == 0)
971  {
972  auto_params_.wb_red = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceRedRegister") / 255.;
973  auto_params_.wb_green = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceGreenRegister") / 255.;
974  auto_params_.wb_blue = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceBlueRegister") / 255.;
975  }
976  // the standard way
977  else if (implemented_features_["BalanceRatio"] && implemented_features_["BalanceRatioSelector"])
978  {
979  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Red");
980  auto_params_.wb_red = arv_device_get_float_feature_value(p_device_, "BalanceRatio");
981  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Green");
982  auto_params_.wb_green = arv_device_get_float_feature_value(p_device_, "BalanceRatio");
983  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Blue");
984  auto_params_.wb_blue = arv_device_get_float_feature_value(p_device_, "BalanceRatio");
985  }
986  }
987 }
988 
989 void CameraAravisNodelet::setAutoMaster(bool value)
990 {
991  if (value)
992  {
993  syncAutoParameters();
994  auto_pub_ = getNodeHandle().advertise<CameraAutoInfo>(ros::names::remap("camera_auto_info"), 1, true);
995  }
996  else
997  {
998  auto_pub_.shutdown();
999  }
1000 }
1001 
1002 void CameraAravisNodelet::setAutoSlave(bool value)
1003 {
1004  if (value)
1005  {
1006  // deactivate all auto functions
1007  if (implemented_features_["ExposureAuto"])
1008  {
1009  arv_device_set_string_feature_value(p_device_, "ExposureAuto", "Off");
1010  }
1011  if (implemented_features_["GainAuto"])
1012  {
1013  arv_device_set_string_feature_value(p_device_, "GainAuto", "Off");
1014  }
1015  if (implemented_features_["GainAutoBalance"])
1016  {
1017  arv_device_set_string_feature_value(p_device_, "GainAutoBalance", "Off");
1018  }
1019  if (implemented_features_["BlackLevelAuto"])
1020  {
1021  arv_device_set_string_feature_value(p_device_, "BlackLevelAuto", "Off");
1022  }
1023  if (implemented_features_["BlackLevelAutoBalance"])
1024  {
1025  arv_device_set_string_feature_value(p_device_, "BlackLevelAutoBalance", "Off");
1026  }
1027  if (implemented_features_["BalanceWhiteAuto"])
1028  {
1029  arv_device_set_string_feature_value(p_device_, "BalanceWhiteAuto", "Off");
1030  }
1031  syncAutoParameters();
1032  auto_sub_ = getNodeHandle().subscribe(ros::names::remap("camera_auto_info"), 1,
1033  &CameraAravisNodelet::cameraAutoInfoCallback, this);
1034  }
1035  else
1036  {
1037  auto_sub_.shutdown();
1038  }
1039 }
1040 
1041 void CameraAravisNodelet::setExtendedCameraInfo(std::string channel_name, size_t stream_id)
1042 {
1043  if (pub_ext_camera_info_)
1044  {
1045  if (channel_name.empty())
1046  {
1047  extended_camera_info_pubs_[stream_id] = getNodeHandle().advertise<ExtendedCameraInfo>(ros::names::remap("extended_camera_info"), 1, true);
1048  }
1049  else
1050  {
1051  extended_camera_info_pubs_[stream_id] = getNodeHandle().advertise<ExtendedCameraInfo>(ros::names::remap(channel_name + "/extended_camera_info"), 1, true);
1052  }
1053  }
1054  else
1055  {
1056  extended_camera_info_pubs_[stream_id].shutdown();
1057  }
1058 }
1059 
1060 // Extra stream options for GigEVision streams.
1061 void CameraAravisNodelet::tuneGvStream(ArvGvStream* p_stream)
1062 {
1063  gboolean b_auto_buffer = FALSE;
1064  gboolean b_packet_resend = TRUE;
1065  unsigned int timeout_packet = 40; // milliseconds
1066  unsigned int timeout_frame_retention = 200;
1067 
1068  if (p_stream)
1069  {
1070  if (!ARV_IS_GV_STREAM(p_stream))
1071  {
1072  ROS_WARN("Stream is not a GV_STREAM");
1073  return;
1074  }
1075 
1076  if (b_auto_buffer)
1077  g_object_set(p_stream, "socket-buffer", ARV_GV_STREAM_SOCKET_BUFFER_AUTO, "socket-buffer-size", 0,
1078  NULL);
1079  if (!b_packet_resend)
1080  g_object_set(p_stream, "packet-resend",
1081  b_packet_resend ? ARV_GV_STREAM_PACKET_RESEND_ALWAYS : ARV_GV_STREAM_PACKET_RESEND_NEVER,
1082  NULL);
1083  g_object_set(p_stream, "packet-timeout", timeout_packet * 1000, "frame-retention", timeout_frame_retention * 1000,
1084  NULL);
1085  }
1086 }
1087 
1088 void CameraAravisNodelet::rosReconfigureCallback(Config& config, uint32_t level)
1089 {
1090  reconfigure_mutex_.lock();
1091  std::string tf_prefix = tf::getPrefixParam(getNodeHandle());
1092  ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
1093 
1094  if (config.frame_id == "")
1095  config.frame_id = this->getName();
1096 
1097  // Limit params to legal values.
1098  config.AcquisitionFrameRate = CLAMP(config.AcquisitionFrameRate, config_min_.AcquisitionFrameRate,
1099  config_max_.AcquisitionFrameRate);
1100  config.ExposureTime = CLAMP(config.ExposureTime, config_min_.ExposureTime, config_max_.ExposureTime);
1101  config.Gain = CLAMP(config.Gain, config_min_.Gain, config_max_.Gain);
1102  config.FocusPos = CLAMP(config.FocusPos, config_min_.FocusPos, config_max_.FocusPos);
1103  config.frame_id = tf::resolve(tf_prefix, config.frame_id);
1104 
1105  if (use_ptp_stamp_)
1106  resetPtpClock();
1107 
1108  // stop auto functions if slave
1109  if (config.AutoSlave)
1110  {
1111  config.ExposureAuto = "Off";
1112  config.GainAuto = "Off";
1113  // todo: all other auto functions "Off"
1114  }
1115 
1116  // reset values controlled by auto functions
1117  if (config.ExposureAuto.compare("Off") != 0)
1118  {
1119  config.ExposureTime = config_.ExposureTime;
1120  ROS_WARN("ExposureAuto is active. Cannot manually set ExposureTime.");
1121  }
1122  if (config.GainAuto.compare("Off") != 0)
1123  {
1124  config.Gain = config_.Gain;
1125  ROS_WARN("GainAuto is active. Cannot manually set Gain.");
1126  }
1127 
1128  // reset FrameRate when triggered
1129  if (config.TriggerMode.compare("Off") != 0)
1130  {
1131  config.AcquisitionFrameRate = config_.AcquisitionFrameRate;
1132  ROS_WARN("TriggerMode is active (Trigger Source: %s). Cannot manually set AcquisitionFrameRate.", config_.TriggerSource.c_str());
1133  }
1134 
1135  // Find valid user changes we need to react to.
1136  const bool changed_auto_master = (config_.AutoMaster != config.AutoMaster);
1137  const bool changed_auto_slave = (config_.AutoSlave != config.AutoSlave);
1138  const bool changed_acquisition_frame_rate = (config_.AcquisitionFrameRate != config.AcquisitionFrameRate);
1139  const bool changed_exposure_auto = (config_.ExposureAuto != config.ExposureAuto);
1140  const bool changed_exposure_time = (config_.ExposureTime != config.ExposureTime);
1141  const bool changed_gain_auto = (config_.GainAuto != config.GainAuto);
1142  const bool changed_gain = (config_.Gain != config.Gain);
1143  const bool changed_acquisition_mode = (config_.AcquisitionMode != config.AcquisitionMode);
1144  const bool changed_trigger_mode = (config_.TriggerMode != config.TriggerMode);
1145  const bool changed_trigger_source = (config_.TriggerSource != config.TriggerSource) || changed_trigger_mode;
1146  const bool changed_focus_pos = (config_.FocusPos != config.FocusPos);
1147  const bool changed_mtu = (config_.mtu != config.mtu);
1148 
1149  if (changed_auto_master)
1150  {
1151  setAutoMaster(config.AutoMaster);
1152  }
1153 
1154  if (changed_auto_slave)
1155  {
1156  setAutoSlave(config.AutoSlave);
1157  }
1158 
1159  // Set params into the camera.
1160  if (changed_exposure_time)
1161  {
1162  if (implemented_features_["ExposureTime"])
1163  {
1164  ROS_INFO("Set ExposureTime = %f us", config.ExposureTime);
1165  arv_camera_set_exposure_time(p_camera_, config.ExposureTime);
1166  }
1167  else
1168  ROS_INFO("Camera does not support ExposureTime.");
1169  }
1170 
1171  if (changed_gain)
1172  {
1173  if (implemented_features_["Gain"])
1174  {
1175  ROS_INFO("Set gain = %f", config.Gain);
1176  arv_camera_set_gain(p_camera_, config.Gain);
1177  }
1178  else
1179  ROS_INFO("Camera does not support Gain or GainRaw.");
1180  }
1181 
1182  if (changed_exposure_auto)
1183  {
1184  if (implemented_features_["ExposureAuto"] && implemented_features_["ExposureTime"])
1185  {
1186  ROS_INFO("Set ExposureAuto = %s", config.ExposureAuto.c_str());
1187  arv_device_set_string_feature_value(p_device_, "ExposureAuto", config.ExposureAuto.c_str());
1188  if (config.ExposureAuto.compare("Once") == 0)
1189  {
1190  ros::Duration(2.0).sleep();
1191  config.ExposureTime = arv_camera_get_exposure_time(p_camera_);
1192  ROS_INFO("Get ExposureTime = %f us", config.ExposureTime);
1193  config.ExposureAuto = "Off";
1194  }
1195  }
1196  else
1197  ROS_INFO("Camera does not support ExposureAuto.");
1198  }
1199  if (changed_gain_auto)
1200  {
1201  if (implemented_features_["GainAuto"] && implemented_features_["Gain"])
1202  {
1203  ROS_INFO("Set GainAuto = %s", config.GainAuto.c_str());
1204  arv_device_set_string_feature_value(p_device_, "GainAuto", config.GainAuto.c_str());
1205  if (config.GainAuto.compare("Once") == 0)
1206  {
1207  ros::Duration(2.0).sleep();
1208  config.Gain = arv_camera_get_gain(p_camera_);
1209  ROS_INFO("Get Gain = %f", config.Gain);
1210  config.GainAuto = "Off";
1211  }
1212  }
1213  else
1214  ROS_INFO("Camera does not support GainAuto.");
1215  }
1216 
1217  if (changed_acquisition_frame_rate)
1218  {
1219  if (implemented_features_["AcquisitionFrameRate"])
1220  {
1221  ROS_INFO("Set frame rate = %f Hz", config.AcquisitionFrameRate);
1222  arv_camera_set_frame_rate(p_camera_, config.AcquisitionFrameRate);
1223  }
1224  else
1225  ROS_INFO("Camera does not support AcquisitionFrameRate.");
1226  }
1227 
1228  if (changed_trigger_mode)
1229  {
1230  if (implemented_features_["TriggerMode"])
1231  {
1232  ROS_INFO("Set TriggerMode = %s", config.TriggerMode.c_str());
1233  arv_device_set_string_feature_value(p_device_, "TriggerMode", config.TriggerMode.c_str());
1234  }
1235  else
1236  ROS_INFO("Camera does not support TriggerMode.");
1237  }
1238 
1239  if (changed_trigger_source)
1240  {
1241  // delete old software trigger thread if active
1242  software_trigger_active_ = false;
1243  if (software_trigger_thread_.joinable())
1244  {
1245  software_trigger_thread_.join();
1246  }
1247 
1248  if (implemented_features_["TriggerSource"])
1249  {
1250  ROS_INFO("Set TriggerSource = %s", config.TriggerSource.c_str());
1251  arv_device_set_string_feature_value(p_device_, "TriggerSource", config.TriggerSource.c_str());
1252  }
1253  else
1254  {
1255  ROS_INFO("Camera does not support TriggerSource.");
1256  }
1257 
1258  // activate on demand
1259  if (config.TriggerMode.compare("On") == 0 && config.TriggerSource.compare("Software") == 0)
1260  {
1261  if (implemented_features_["TriggerSoftware"])
1262  {
1263  config_.softwaretriggerrate = config.softwaretriggerrate;
1264  ROS_INFO("Set softwaretriggerrate = %f", 1000.0 / ceil(1000.0 / config.softwaretriggerrate));
1265 
1266  // Turn on software timer callback.
1267  software_trigger_thread_ = std::thread(&CameraAravisNodelet::softwareTriggerLoop, this);
1268  }
1269  else
1270  {
1271  ROS_INFO("Camera does not support TriggerSoftware command.");
1272  }
1273  }
1274  }
1275 
1276  if (changed_focus_pos)
1277  {
1278  if (implemented_features_["FocusPos"])
1279  {
1280  ROS_INFO("Set FocusPos = %d", config.FocusPos);
1281  arv_device_set_integer_feature_value(p_device_, "FocusPos", config.FocusPos);
1282  ros::Duration(1.0).sleep();
1283  config.FocusPos = arv_device_get_integer_feature_value(p_device_, "FocusPos");
1284  ROS_INFO("Get FocusPos = %d", config.FocusPos);
1285  }
1286  else
1287  ROS_INFO("Camera does not support FocusPos.");
1288  }
1289 
1290  if (changed_mtu)
1291  {
1292  if (implemented_features_["GevSCPSPacketSize"])
1293  {
1294  ROS_INFO("Set mtu = %d", config.mtu);
1295  arv_device_set_integer_feature_value(p_device_, "GevSCPSPacketSize", config.mtu);
1296  ros::Duration(1.0).sleep();
1297  config.mtu = arv_device_get_integer_feature_value(p_device_, "GevSCPSPacketSize");
1298  ROS_INFO("Get mtu = %d", config.mtu);
1299  }
1300  else
1301  ROS_INFO("Camera does not support mtu (i.e. GevSCPSPacketSize).");
1302  }
1303 
1304  if (changed_acquisition_mode)
1305  {
1306  if (implemented_features_["AcquisitionMode"])
1307  {
1308  ROS_INFO("Set AcquisitionMode = %s", config.AcquisitionMode.c_str());
1309  arv_device_set_string_feature_value(p_device_, "AcquisitionMode", config.AcquisitionMode.c_str());
1310 
1311  ROS_INFO("AcquisitionStop");
1312  arv_device_execute_command(p_device_, "AcquisitionStop");
1313  ROS_INFO("AcquisitionStart");
1314  arv_device_execute_command(p_device_, "AcquisitionStart");
1315  }
1316  else
1317  ROS_INFO("Camera does not support AcquisitionMode.");
1318  }
1319 
1320  // adopt new config
1321  config_ = config;
1322  reconfigure_mutex_.unlock();
1323 }
1324 
1325 void CameraAravisNodelet::rosConnectCallback()
1326 {
1327  if (p_device_)
1328  {
1329  if (std::all_of(cam_pubs_.begin(), cam_pubs_.end(),
1330  [](image_transport::CameraPublisher pub)
1331  { return pub.getNumSubscribers() == 0; }))
1332  {
1333  arv_device_execute_command(p_device_, "AcquisitionStop"); // don't waste CPU if nobody is listening!
1334  }
1335  else
1336  {
1337  arv_device_execute_command(p_device_, "AcquisitionStart");
1338  }
1339  }
1340 }
1341 
1342 void CameraAravisNodelet::newBufferReadyCallback(ArvStream* p_stream, gpointer can_instance)
1343 {
1344 
1345  // workaround to get access to the instance from a static method
1346  StreamIdData* data = (StreamIdData*)can_instance;
1347  CameraAravisNodelet* p_can = (CameraAravisNodelet*)data->can;
1348  size_t stream_id = data->stream_id;
1349  image_transport::CameraPublisher* p_cam_pub = &p_can->cam_pubs_[stream_id];
1350 
1351  if (p_can->stream_names_[stream_id].empty())
1352  {
1353  newBufferReady(p_stream, p_can, p_can->config_.frame_id, stream_id);
1354  }
1355  else
1356  {
1357  const std::string stream_frame_id = p_can->config_.frame_id + "/" + p_can->stream_names_[stream_id];
1358  newBufferReady(p_stream, p_can, stream_frame_id, stream_id);
1359  }
1360 }
1361 
1362 void CameraAravisNodelet::newBufferReady(ArvStream* p_stream, CameraAravisNodelet* p_can, std::string frame_id, size_t stream_id)
1363 {
1364  ArvBuffer* p_buffer = arv_stream_try_pop_buffer(p_stream);
1365 
1366  // check if we risk to drop the next image because of not enough buffers left
1367  gint n_available_buffers;
1368  arv_stream_get_n_buffers(p_stream, &n_available_buffers, NULL);
1369  if (n_available_buffers == 0)
1370  {
1371  p_can->p_buffer_pools_[stream_id]->allocateBuffers(1);
1372  }
1373 
1374  if (p_buffer != NULL)
1375  {
1376  if (arv_buffer_get_status(p_buffer) == ARV_BUFFER_STATUS_SUCCESS && p_can->p_buffer_pools_[stream_id] && p_can->cam_pubs_[stream_id].getNumSubscribers())
1377  {
1378  (p_can->n_buffers_)++;
1379  // get the image message which wraps around this buffer
1380  sensor_msgs::ImagePtr msg_ptr = (*(p_can->p_buffer_pools_[stream_id]))[p_buffer];
1381  // fill the meta information of image message
1382  // get acquisition time
1383  guint64 t;
1384  if (p_can->use_ptp_stamp_)
1385  t = arv_buffer_get_timestamp(p_buffer);
1386  else
1387  t = arv_buffer_get_system_timestamp(p_buffer);
1388  msg_ptr->header.stamp.fromNSec(t);
1389  // get frame sequence number
1390  msg_ptr->header.seq = arv_buffer_get_frame_id(p_buffer);
1391  // fill other stream properties
1392  msg_ptr->header.frame_id = frame_id;
1393  msg_ptr->width = p_can->roi_.width;
1394  msg_ptr->height = p_can->roi_.height;
1395  msg_ptr->encoding = p_can->sensors_[stream_id]->pixel_format;
1396  msg_ptr->step = (msg_ptr->width * p_can->sensors_[stream_id]->n_bits_pixel) / 8;
1397 
1398  // do the magic of conversion into a ROS format
1399  if (p_can->convert_formats[stream_id])
1400  {
1401  sensor_msgs::ImagePtr cvt_msg_ptr = p_can->p_buffer_pools_[stream_id]->getRecyclableImg();
1402  p_can->convert_formats[stream_id](msg_ptr, cvt_msg_ptr);
1403  msg_ptr = cvt_msg_ptr;
1404  }
1405 
1406  // get current CameraInfo data
1407  if (!p_can->camera_infos_[stream_id])
1408  {
1409  p_can->camera_infos_[stream_id].reset(new sensor_msgs::CameraInfo);
1410  }
1411  (*p_can->camera_infos_[stream_id]) = p_can->p_camera_info_managers_[stream_id]->getCameraInfo();
1412  p_can->camera_infos_[stream_id]->header = msg_ptr->header;
1413  if (p_can->camera_infos_[stream_id]->width == 0 || p_can->camera_infos_[stream_id]->height == 0)
1414  {
1415  ROS_WARN_STREAM_ONCE(
1416  "The fields image_width and image_height seem not to be set in "
1417  "the YAML specified by 'camera_info_url' parameter. Please set "
1418  "them there, because actual image size and specified image size "
1419  "can be different due to the region of interest (ROI) feature. In "
1420  "the YAML the image size should be the one on which the camera was "
1421  "calibrated. See CameraInfo.msg specification!");
1422  p_can->camera_infos_[stream_id]->width = p_can->roi_.width;
1423  p_can->camera_infos_[stream_id]->height = p_can->roi_.height;
1424  }
1425 
1426  p_can->cam_pubs_[stream_id].publish(msg_ptr, p_can->camera_infos_[stream_id]);
1427 
1428  if (p_can->pub_ext_camera_info_)
1429  {
1430  ExtendedCameraInfo extended_camera_info_msg;
1431  p_can->extended_camera_info_mutex_.lock();
1432  arv_camera_gv_select_stream_channel(p_can->p_camera_, stream_id);
1433  extended_camera_info_msg.camera_info = *(p_can->camera_infos_[stream_id]);
1434  p_can->fillExtendedCameraInfoMessage(extended_camera_info_msg);
1435  p_can->extended_camera_info_mutex_.unlock();
1436  p_can->extended_camera_info_pubs_[stream_id].publish(extended_camera_info_msg);
1437  }
1438 
1439  // check PTP status, camera cannot recover from "Faulty" by itself
1440  if (p_can->use_ptp_stamp_)
1441  p_can->resetPtpClock();
1442  }
1443  else
1444  {
1445  if (arv_buffer_get_status(p_buffer) != ARV_BUFFER_STATUS_SUCCESS)
1446  {
1447  ROS_WARN("(%s) Frame error: %s", frame_id.c_str(), szBufferStatusFromInt[arv_buffer_get_status(p_buffer)]);
1448  }
1449  arv_stream_push_buffer(p_stream, p_buffer);
1450  }
1451  }
1452 
1453  // publish current lighting settings if this camera is configured as master
1454  if (p_can->config_.AutoMaster)
1455  {
1456  p_can->syncAutoParameters();
1457  p_can->auto_pub_.publish(p_can->auto_params_);
1458  }
1459 }
1460 
1461 void CameraAravisNodelet::fillExtendedCameraInfoMessage(ExtendedCameraInfo& msg)
1462 {
1463  const char* vendor_name = arv_camera_get_vendor_name(p_camera_);
1464 
1465  if (strcmp("Basler", vendor_name) == 0)
1466  {
1467  msg.exposure_time = arv_device_get_float_feature_value(p_device_, "ExposureTimeAbs");
1468  }
1469  else if (implemented_features_["ExposureTime"])
1470  {
1471  msg.exposure_time = arv_device_get_float_feature_value(p_device_, "ExposureTime");
1472  }
1473 
1474  if (strcmp("Basler", vendor_name) == 0)
1475  {
1476  msg.gain = static_cast<float>(arv_device_get_integer_feature_value(p_device_, "GainRaw"));
1477  }
1478  else if (implemented_features_["Gain"])
1479  {
1480  msg.gain = arv_device_get_float_feature_value(p_device_, "Gain");
1481  }
1482  if (strcmp("Basler", vendor_name) == 0)
1483  {
1484  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "All");
1485  msg.black_level = static_cast<float>(arv_device_get_integer_feature_value(p_device_, "BlackLevelRaw"));
1486  }
1487  else if (strcmp("JAI Corporation", vendor_name) == 0)
1488  {
1489  // Reading the black level register for both streams of the JAI FS 3500D takes too long.
1490  // The frame rate the drops below 10 fps.
1491  msg.black_level = 0;
1492  }
1493  else
1494  {
1495  arv_device_set_string_feature_value(p_device_, "BlackLevelSelector", "All");
1496  msg.black_level = arv_device_get_float_feature_value(p_device_, "BlackLevel");
1497  }
1498 
1499  // White balance as TIS is providing
1500  if (strcmp("The Imaging Source Europe GmbH", vendor_name) == 0)
1501  {
1502  msg.white_balance_red = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceRedRegister") / 255.;
1503  msg.white_balance_green = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceGreenRegister") / 255.;
1504  msg.white_balance_blue = arv_device_get_integer_feature_value(p_device_, "WhiteBalanceBlueRegister") / 255.;
1505  }
1506  // the JAI cameras become too slow when reading out the DigitalRed and DigitalBlue values
1507  // the white balance is adjusted by adjusting the Gain values for Red and Blue pixels
1508  else if (strcmp("JAI Corporation", vendor_name) == 0)
1509  {
1510  msg.white_balance_red = 1.0;
1511  msg.white_balance_green = 1.0;
1512  msg.white_balance_blue = 1.0;
1513  }
1514  // the Basler cameras use the 'BalanceRatioAbs' keyword instead
1515  else if (strcmp("Basler", vendor_name) == 0)
1516  {
1517  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Red");
1518  msg.white_balance_red = arv_device_get_float_feature_value(p_device_, "BalanceRatioAbs");
1519  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Green");
1520  msg.white_balance_green = arv_device_get_float_feature_value(p_device_, "BalanceRatioAbs");
1521  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Blue");
1522  msg.white_balance_blue = arv_device_get_float_feature_value(p_device_, "BalanceRatioAbs");
1523  }
1524  // the standard way
1525  else if (implemented_features_["BalanceRatio"] && implemented_features_["BalanceRatioSelector"])
1526  {
1527  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Red");
1528  msg.white_balance_red = arv_device_get_float_feature_value(p_device_, "BalanceRatio");
1529  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Green");
1530  msg.white_balance_green = arv_device_get_float_feature_value(p_device_, "BalanceRatio");
1531  arv_device_set_string_feature_value(p_device_, "BalanceRatioSelector", "Blue");
1532  msg.white_balance_blue = arv_device_get_float_feature_value(p_device_, "BalanceRatio");
1533  }
1534 
1535  if (strcmp("Basler", vendor_name) == 0)
1536  {
1537  msg.temperature = static_cast<float>(arv_device_get_float_feature_value(p_device_, "TemperatureAbs"));
1538  }
1539  else if (implemented_features_["DeviceTemperature"])
1540  {
1541  msg.temperature = arv_device_get_float_feature_value(p_device_, "DeviceTemperature");
1542  }
1543 }
1544 
1545 void CameraAravisNodelet::controlLostCallback(ArvDevice* p_gv_device, gpointer can_instance)
1546 {
1547  // CameraAravisNodelet *p_can = (CameraAravisNodelet*)can_instance;
1548  // ROS_ERROR("Control to aravis device lost.\n\t> Nodelet name: %s."
1549  // "\n\t> Shutting down. Allowing for respawn.",
1550  // p_can->getName().c_str());
1551 
1552  // p_can->shutdown_trigger_.start();
1553 }
1554 
1555 void CameraAravisNodelet::softwareTriggerLoop()
1556 {
1557  software_trigger_active_ = true;
1558  ROS_INFO("Software trigger started.");
1559  std::chrono::system_clock::time_point next_time = std::chrono::system_clock::now();
1560  while (ros::ok() && software_trigger_active_)
1561  {
1562  next_time += std::chrono::milliseconds(size_t(std::round(1000.0 / config_.softwaretriggerrate)));
1563  if (std::any_of(cam_pubs_.begin(), cam_pubs_.end(),
1564  [](image_transport::CameraPublisher pub)
1565  { return pub.getNumSubscribers() > 0; }))
1566  {
1567  arv_device_execute_command(p_device_, "TriggerSoftware");
1568  }
1569  if (next_time > std::chrono::system_clock::now())
1570  {
1571  std::this_thread::sleep_until(next_time);
1572  }
1573  else
1574  {
1575  ROS_WARN("Camera Aravis: Missed a software trigger event.");
1576  next_time = std::chrono::system_clock::now();
1577  }
1578  }
1579  ROS_INFO("Software trigger stopped.");
1580 }
1581 
1582 void CameraAravisNodelet::publishTfLoop(double rate)
1583 {
1584  // Publish optical transform for the camera
1585  ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", rate);
1586 
1587  tf_thread_active_ = true;
1588 
1589  ros::Rate loop_rate(rate);
1590 
1591  while (ros::ok() && tf_thread_active_)
1592  {
1593  // Update the header for publication
1594  tf_optical_.header.stamp = ros::Time::now();
1595  ++tf_optical_.header.seq;
1596  p_tb_->sendTransform(tf_optical_);
1597 
1598  loop_rate.sleep();
1599  }
1600 }
1601 
1602 void CameraAravisNodelet::readAndPublishCameraDiagnostics(double rate) const
1603 {
1604  ROS_INFO("Publishing camera diagnostics at %g Hz", rate);
1605 
1606  ros::Rate loop_rate(rate);
1607 
1608  CameraDiagnostics camDiagnosticMsg;
1609  camDiagnosticMsg.header.frame_id = config_.frame_id;
1610 
1611  // function to get feature value at given name and of given type as string
1612  auto getFeatureValueAsStrFn = [&](const std::string& name, const std::string& type)
1613  -> std::string
1614  {
1615  std::string valueStr = "";
1616 
1617  if (type == "float")
1618  {
1619  valueStr = std::to_string(arv_device_get_float_feature_value(p_device_, name.c_str()));
1620  }
1621  else if (type == "int")
1622  {
1623  valueStr = std::to_string(arv_device_get_integer_feature_value(p_device_, name.c_str()));
1624  }
1625  else if (type == "bool")
1626  {
1627  valueStr = arv_device_get_boolean_feature_value(p_device_, name.c_str()) ? "true" : "false";
1628  }
1629  else
1630  {
1631  const char* retFeatVal = arv_device_get_string_feature_value(p_device_, name.c_str());
1632  valueStr = (retFeatVal) ? std::string(retFeatVal) : "";
1633  }
1634 
1635  return valueStr;
1636  };
1637 
1638  // function to set feature value at given name and of given type from string
1639  auto setFeatureValueFromStrFn = [&](const std::string& name, const std::string& type,
1640  const std::string& valueStr)
1641  {
1642  if (type == "float")
1643  arv_device_set_float_feature_value(p_device_, name.c_str(), std::stod(valueStr));
1644  else if (type == "int")
1645  arv_device_set_integer_feature_value(p_device_, name.c_str(), std::stoi(valueStr));
1646  else if (type == "bool")
1647  arv_device_set_boolean_feature_value(p_device_, name.c_str(),
1648  (valueStr == "true" || valueStr == "True" || valueStr == "TRUE"));
1649  else
1650  arv_device_set_string_feature_value(p_device_, name.c_str(), valueStr.c_str());
1651  };
1652 
1653  while (ros::ok() && diagnostic_thread_active_)
1654  {
1655  camDiagnosticMsg.header.stamp = ros::Time::now();
1656  ++camDiagnosticMsg.header.seq;
1657 
1658  camDiagnosticMsg.data.clear();
1659 
1660  int featureIdx = 1;
1661  for (auto featureDict : diagnostic_features_)
1662  {
1663  std::string featureName = featureDict["FeatureName"].IsDefined()
1664  ? featureDict["FeatureName"].as<std::string>()
1665  : "";
1666  std::string featureType = featureDict["Type"].IsDefined()
1667  ? featureDict["Type"].as<std::string>()
1668  : "";
1669 
1670  if (featureName.empty() || featureType.empty())
1671  {
1672  ROS_WARN_ONCE(
1673  "Diagnostic feature at index %i does not have a field 'FeatureName' or 'Type'.",
1674  featureIdx);
1675  ROS_WARN_ONCE("Diagnostic feature will be skipped.");
1676  }
1677  else
1678  {
1679  // convert type string to lower case
1680  std::transform(featureType.begin(), featureType.end(), featureType.begin(),
1681  [](unsigned char c)
1682  { return std::tolower(c); });
1683 
1684  // if feature name is found in implemented_feature list and if enabled
1685  if (implemented_features_.find(featureName) != implemented_features_.end() &&
1686  implemented_features_.at(featureName))
1687  {
1688  diagnostic_msgs::KeyValue kvPair;
1689 
1690  // if 'selectors' which correspond to the featureName are defined
1691  if (featureDict["Selectors"].IsDefined() && featureDict["Selectors"].size() > 0)
1692  {
1693  int selectorIdx = 1;
1694  for (auto selectorDict : featureDict["Selectors"])
1695  {
1696  std::string selectorFeatureName = selectorDict["FeatureName"].IsDefined()
1697  ? selectorDict["FeatureName"].as<std::string>()
1698  : "";
1699  std::string selectorType = selectorDict["Type"].IsDefined()
1700  ? selectorDict["Type"].as<std::string>()
1701  : "";
1702  std::string selectorValue = selectorDict["Value"].IsDefined()
1703  ? selectorDict["Value"].as<std::string>()
1704  : "";
1705 
1706  if (selectorFeatureName.empty() || selectorType.empty() || selectorValue.empty())
1707  {
1708  ROS_WARN_ONCE(
1709  "Diagnostic feature selector at index %i of feature at index %i does not have a "
1710  "field 'FeatureName', 'Type' or 'Value'.",
1711  selectorIdx, featureIdx);
1712  ROS_WARN_ONCE("Diagnostic feature will be skipped.");
1713  }
1714  else
1715  {
1716  if (implemented_features_.find(selectorFeatureName) != implemented_features_.end() &&
1717  implemented_features_.at(selectorFeatureName))
1718  {
1719  setFeatureValueFromStrFn(selectorFeatureName, selectorType, selectorValue);
1720 
1721  kvPair.key = featureName + "-" + selectorValue;
1722  kvPair.value = getFeatureValueAsStrFn(featureName, featureType);
1723 
1724  camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair));
1725  }
1726  else
1727  {
1728  ROS_WARN_ONCE("Diagnostic feature selector with name '%s' is not implemented.",
1729  selectorFeatureName.c_str());
1730  }
1731 
1732  selectorIdx++;
1733  }
1734  }
1735  }
1736  else
1737  {
1738  kvPair.key = featureName;
1739  kvPair.value = getFeatureValueAsStrFn(featureName, featureType);
1740 
1741  camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair));
1742  }
1743  }
1744  else
1745  {
1746  ROS_WARN_ONCE("Diagnostic feature with name '%s' is not implemented.",
1747  featureName.c_str());
1748  }
1749  }
1750 
1751  featureIdx++;
1752  }
1753 
1754  diagnostic_pub_.publish(camDiagnosticMsg);
1755 
1756  loop_rate.sleep();
1757  }
1758 }
1759 
1760 void CameraAravisNodelet::discoverFeatures()
1761 {
1762  implemented_features_.clear();
1763  if (!p_device_)
1764  return;
1765 
1766  // get the root node of genicam description
1767  ArvGc* gc = arv_device_get_genicam(p_device_);
1768  if (!gc)
1769  return;
1770 
1771  std::list<ArvDomNode*> todo;
1772  todo.push_front((ArvDomNode*)arv_gc_get_node(gc, "Root"));
1773 
1774  while (!todo.empty())
1775  {
1776  // get next entry
1777  ArvDomNode* node = todo.front();
1778  todo.pop_front();
1779  const std::string name(arv_dom_node_get_node_name(node));
1780 
1781  // Do the indirection
1782  if (name[0] == 'p')
1783  {
1784  if (name.compare("pInvalidator") == 0)
1785  {
1786  continue;
1787  }
1788  ArvDomNode* inode = (ArvDomNode*)arv_gc_get_node(gc,
1789  arv_dom_node_get_node_value(arv_dom_node_get_first_child(node)));
1790  if (inode)
1791  todo.push_front(inode);
1792  continue;
1793  }
1794 
1795  // check for implemented feature
1796  if (ARV_IS_GC_FEATURE_NODE(node))
1797  {
1798  // if (!(ARV_IS_GC_CATEGORY(node) || ARV_IS_GC_ENUM_ENTRY(node) /*|| ARV_IS_GC_PORT(node)*/)) {
1799  ArvGcFeatureNode* fnode = ARV_GC_FEATURE_NODE(node);
1800  const std::string fname(arv_gc_feature_node_get_name(fnode));
1801  const bool usable = arv_gc_feature_node_is_available(fnode, NULL) && arv_gc_feature_node_is_implemented(fnode, NULL);
1802 
1803  ROS_INFO_STREAM_COND(verbose_, "Feature " << fname << " is " << (usable ? "usable" : "not usable"));
1804  implemented_features_.emplace(fname, usable);
1805  //}
1806  }
1807 
1808  // if (ARV_IS_GC_PROPERTY_NODE(node)) {
1809  // ArvGcPropertyNode* pnode = ARV_GC_PROPERTY_NODE(node);
1810  // const std::string pname(arv_gc_property_node_get_name(pnode));
1811  // ROS_INFO_STREAM("Property " << pname << " found");
1812  // }
1813 
1814  // add children in todo-list
1815  ArvDomNodeList* children = arv_dom_node_get_child_nodes(node);
1816  const uint l = arv_dom_node_list_get_length(children);
1817  for (uint i = 0; i < l; ++i)
1818  {
1819  todo.push_front(arv_dom_node_list_get_item(children, i));
1820  }
1821  }
1822 }
1823 
1824 void CameraAravisNodelet::parseStringArgs(std::string in_arg_string, std::vector<std::string>& out_args)
1825 {
1826  size_t array_start = 0;
1827  size_t array_end = in_arg_string.length();
1828  if (array_start != std::string::npos && array_end != std::string::npos)
1829  {
1830  // parse the string into an array of parameters
1831  std::stringstream ss(in_arg_string.substr(array_start, array_end - array_start));
1832  while (ss.good())
1833  {
1834  std::string temp;
1835  getline(ss, temp, ',');
1836  boost::trim_left(temp);
1837  boost::trim_right(temp);
1838  out_args.push_back(temp);
1839  }
1840  }
1841  else
1842  {
1843  // add just the one argument onto the vector
1844  out_args.push_back(in_arg_string);
1845  }
1846 }
1847 
1848 // WriteCameraFeaturesFromRosparam()
1849 // Read ROS parameters from this node's namespace, and see if each parameter has a similarly named & typed feature in the camera. Then set the
1850 // camera feature to that value. For example, if the parameter camnode/Gain is set to 123.0, then we'll write 123.0 to the Gain feature
1851 // in the camera.
1852 //
1853 // Note that the datatype of the parameter *must* match the datatype of the camera feature, and this can be determined by
1854 // looking at the camera's XML file. Camera enum's are string parameters, camera bools are false/true parameters (not 0/1),
1855 // integers are integers, doubles are doubles, etc.
1856 //
1857 void CameraAravisNodelet::writeCameraFeaturesFromRosparam()
1858 {
1859  XmlRpc::XmlRpcValue xml_rpc_params;
1860  XmlRpc::XmlRpcValue::iterator iter;
1861  ArvGcNode* p_gc_node;
1862  GError* error = NULL;
1863 
1864  getPrivateNodeHandle().getParam(this->getName(), xml_rpc_params);
1865 
1866  if (xml_rpc_params.getType() == XmlRpc::XmlRpcValue::TypeStruct)
1867  {
1868  for (iter = xml_rpc_params.begin(); iter != xml_rpc_params.end(); iter++)
1869  {
1870  std::string key = iter->first;
1871 
1872  p_gc_node = arv_device_get_feature(p_device_, key.c_str());
1873  if (p_gc_node && arv_gc_feature_node_is_implemented(ARV_GC_FEATURE_NODE(p_gc_node), &error))
1874  {
1875  // unsigned long typeValue = arv_gc_feature_node_get_value_type((ArvGcFeatureNode *)pGcNode);
1876  // ROS_INFO("%s cameratype=%lu, rosparamtype=%d", key.c_str(), typeValue, static_cast<int>(iter->second.getType()));
1877 
1878  // We'd like to check the value types too, but typeValue is often given as G_TYPE_INVALID, so ignore it.
1879  switch (iter->second.getType())
1880  {
1881  case XmlRpc::XmlRpcValue::TypeBoolean: // if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeBoolean))// && (typeValue==G_TYPE_INT64))
1882  {
1883  int value = (bool)iter->second;
1884  arv_device_set_integer_feature_value(p_device_, key.c_str(), value);
1885  ROS_INFO("Read parameter (bool) %s: %d", key.c_str(), value);
1886  }
1887  break;
1888 
1889  case XmlRpc::XmlRpcValue::TypeInt: // if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeInt))// && (typeValue==G_TYPE_INT64))
1890  {
1891  int value = (int)iter->second;
1892  arv_device_set_integer_feature_value(p_device_, key.c_str(), value);
1893  ROS_INFO("Read parameter (int) %s: %d", key.c_str(), value);
1894  }
1895  break;
1896 
1897  case XmlRpc::XmlRpcValue::TypeDouble: // if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeDouble))// && (typeValue==G_TYPE_DOUBLE))
1898  {
1899  double value = (double)iter->second;
1900  arv_device_set_float_feature_value(p_device_, key.c_str(), value);
1901  ROS_INFO("Read parameter (float) %s: %f", key.c_str(), value);
1902  }
1903  break;
1904 
1905  case XmlRpc::XmlRpcValue::TypeString: // if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeString))// && (typeValue==G_TYPE_STRING))
1906  {
1907  std::string value = (std::string)iter->second;
1908  arv_device_set_string_feature_value(p_device_, key.c_str(), value.c_str());
1909  ROS_INFO("Read parameter (string) %s: %s", key.c_str(), value.c_str());
1910  }
1911  break;
1912 
1913  case XmlRpc::XmlRpcValue::TypeInvalid:
1914  case XmlRpc::XmlRpcValue::TypeDateTime:
1915  case XmlRpc::XmlRpcValue::TypeBase64:
1916  case XmlRpc::XmlRpcValue::TypeArray:
1917  case XmlRpc::XmlRpcValue::TypeStruct:
1918  default:
1919  ROS_WARN("Unhandled rosparam type in writeCameraFeaturesFromRosparam()");
1920  }
1921  }
1922  }
1923  }
1924 }
1925 
1926 void CameraAravisNodelet::onShutdownTriggered(const ros::TimerEvent&)
1927 {
1928  nodelet::NodeletUnload unload_service;
1929  unload_service.request.name = this->getName();
1930  ros::service::call(this->getName() + "/unload_nodelet", unload_service);
1931  ROS_INFO("Nodelet unloaded.");
1932 
1933  ros::Duration(shutdown_delay_s_).sleep();
1934 
1935  ros::shutdown();
1936  ROS_INFO("Shut down successful.");
1937 }
1938 
1939 } // end namespace camera_aravis
camera_aravis::CameraAravisNodelet::ptp_status_feature_
std::string ptp_status_feature_
Definition: camera_aravis_nodelet.h:119
camera_aravis::CameraAravisNodelet::tf_thread_active_
std::atomic_bool tf_thread_active_
Definition: camera_aravis_nodelet.h:230
response
const std::string response
camera_aravis::CameraAravisNodelet::auto_sub_
ros::Subscriber auto_sub_
Definition: camera_aravis_nodelet.h:241
camera_aravis::CameraAravisNodelet::setStringFeatureCallback
bool setStringFeatureCallback(camera_aravis::set_string_feature_value::Request &request, camera_aravis::set_string_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:757
camera_aravis::CameraAravisNodelet::tf_dyn_thread_
std::thread tf_dyn_thread_
Definition: camera_aravis_nodelet.h:229
camera_aravis::CameraAravisNodelet::rosConnectCallback
void rosConnectCallback()
Definition: camera_aravis_nodelet.cpp:1346
camera_aravis::CameraAravisNodelet::shutdown_delay_s_
double shutdown_delay_s_
Definition: camera_aravis_nodelet.h:260
camera_aravis::CameraAravisNodelet::diagnostic_publish_rate_
double diagnostic_publish_rate_
Definition: camera_aravis_nodelet.h:233
camera_aravis::CameraAravisNodelet::newBufferReady
static void newBufferReady(ArvStream *p_stream, CameraAravisNodelet *p_can, std::string frame_id, size_t stream_id)
Definition: camera_aravis_nodelet.cpp:1383
camera_aravis::CameraAravisNodelet::width
int32_t width
Definition: camera_aravis_nodelet.h:266
ros::service::call
bool call(const std::string &service_name, MReq &req, MRes &res)
camera_aravis::CameraAravisNodelet::diagnostic_thread_
std::thread diagnostic_thread_
Definition: camera_aravis_nodelet.h:236
ros::Publisher
camera_aravis::CameraAravisNodelet::fillExtendedCameraInfoMessage
void fillExtendedCameraInfoMessage(ExtendedCameraInfo &msg)
Definition: camera_aravis_nodelet.cpp:1482
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
camera_aravis::CameraAravisNodelet::setExtendedCameraInfo
void setExtendedCameraInfo(std::string channel_name, size_t stream_id)
Definition: camera_aravis_nodelet.cpp:1062
image_transport::ImageTransport
camera_aravis::CameraAravisNodelet::p_tb_
std::unique_ptr< tf2_ros::TransformBroadcaster > p_tb_
Definition: camera_aravis_nodelet.h:227
camera_aravis::CameraAravisNodelet::get_integer_service_
ros::ServiceServer get_integer_service_
Definition: camera_aravis_nodelet.h:171
XmlRpc::XmlRpcValue::TypeBase64
TypeBase64
camera_aravis::CameraAravisNodelet::tf_optical_
geometry_msgs::TransformStamped tf_optical_
Definition: camera_aravis_nodelet.h:228
camera_aravis::CameraAravisNodelet::StreamIdData::can
CameraAravisNodelet * can
Definition: camera_aravis_nodelet.h:286
camera_aravis::CameraAravisNodelet::config_min_
Config config_min_
Definition: camera_aravis_nodelet.h:247
camera_aravis::CameraAravisNodelet::set_integer_service_
ros::ServiceServer set_integer_service_
Definition: camera_aravis_nodelet.h:183
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
ROS_INFO_STREAM_COND
#define ROS_INFO_STREAM_COND(cond, args)
camera_aravis::CameraAravisNodelet::~CameraAravisNodelet
virtual ~CameraAravisNodelet()
Definition: camera_aravis_nodelet.cpp:59
camera_aravis::szBufferStatusFromInt
static const char * szBufferStatusFromInt[]
Definition: conversion_utils.h:57
camera_aravis::CameraAravisNodelet::extended_camera_info_pubs_
std::vector< ros::Publisher > extended_camera_info_pubs_
Definition: camera_aravis_nodelet.h:244
ros::spinOnce
ROSCPP_DECL void spinOnce()
XmlRpc::XmlRpcValue::TypeInt
TypeInt
XmlRpc::XmlRpcValue::TypeInvalid
TypeInvalid
camera_aravis::CameraAravisNodelet::software_trigger_thread_
std::thread software_trigger_thread_
Definition: camera_aravis_nodelet.h:253
camera_aravis::CameraAravisNodelet::StreamIdData::stream_id
size_t stream_id
Definition: camera_aravis_nodelet.h:287
ros::shutdown
ROSCPP_DECL void shutdown()
tf::resolve
std::string resolve(const std::string &prefix, const std::string &frame_name)
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
camera_aravis::CameraAravisNodelet::convert_formats
std::vector< ConversionFunction > convert_formats
Definition: camera_aravis_nodelet.h:133
camera_aravis::CameraAravisNodelet::syncAutoParameters
void syncAutoParameters()
Definition: camera_aravis_nodelet.cpp:940
camera_aravis::Config
CameraAravisConfig Config
Definition: camera_aravis_nodelet.h:105
camera_aravis
Definition: camera_aravis_nodelet.h:81
ros::Subscriber::shutdown
void shutdown()
ROS_WARN_ONCE
#define ROS_WARN_ONCE(...)
camera_aravis::CameraAravisNodelet::get_boolean_service_
ros::ServiceServer get_boolean_service_
Definition: camera_aravis_nodelet.h:180
camera_aravis::CameraAravisNodelet::guid_
std::string guid_
Definition: camera_aravis_nodelet.h:115
camera_aravis::CameraAravisNodelet::p_streams_
std::vector< ArvStream * > p_streams_
Definition: camera_aravis_nodelet.h:129
camera_aravis::CameraAravisNodelet::pub_tf_optical_
bool pub_tf_optical_
Definition: camera_aravis_nodelet.h:123
camera_aravis::CameraAravisNodelet::getIntegerFeatureCallback
bool getIntegerFeatureCallback(camera_aravis::get_integer_feature_value::Request &request, camera_aravis::get_integer_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:704
camera_aravis::CameraAravisNodelet::p_camera_
ArvCamera * p_camera_
Definition: camera_aravis_nodelet.h:125
ros::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
camera_aravis::CameraAravisNodelet::diagnostic_pub_
ros::Publisher diagnostic_pub_
Definition: camera_aravis_nodelet.h:235
camera_aravis::CameraAravisNodelet::ptp_enable_feature_
std::string ptp_enable_feature_
Definition: camera_aravis_nodelet.h:118
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
image_transport::ImageTransport::advertiseCamera
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
camera_aravis::CameraAravisNodelet::CameraAravisNodelet
CameraAravisNodelet()
Definition: camera_aravis_nodelet.cpp:55
camera_aravis::CameraAravisNodelet::spawnStream
void spawnStream()
Definition: camera_aravis_nodelet.cpp:609
camera_aravis::CameraAravisNodelet
Definition: camera_aravis_nodelet.h:107
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
ROS_WARN_STREAM_ONCE
#define ROS_WARN_STREAM_ONCE(args)
camera_aravis::CameraAravisNodelet::getStringFeatureCallback
bool getStringFeatureCallback(camera_aravis::get_string_feature_value::Request &request, camera_aravis::get_string_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:750
XmlRpc::XmlRpcValue::TypeStruct
TypeStruct
camera_aravis::CameraAravisNodelet::getBooleanFeatureCallback
bool getBooleanFeatureCallback(camera_aravis::get_boolean_feature_value::Request &request, camera_aravis::get_boolean_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:773
camera_aravis::CameraAravisNodelet::publishTfLoop
void publishTfLoop(double rate)
Definition: camera_aravis_nodelet.cpp:1603
camera_aravis::CameraAravisNodelet::diagnostic_yaml_url_
std::string diagnostic_yaml_url_
Definition: camera_aravis_nodelet.h:232
class_list_macros.h
camera_aravis::CameraAravisNodelet::shutdown_trigger_
ros::Timer shutdown_trigger_
Definition: camera_aravis_nodelet.h:259
tf2_ros::StaticTransformBroadcaster
camera_aravis::CameraAravisNodelet::sensors_
std::vector< Sensor * > sensors_
Definition: camera_aravis_nodelet.h:282
ros::SingleSubscriberPublisher
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
camera_aravis::CameraAravisNodelet::camera_infos_
std::vector< sensor_msgs::CameraInfoPtr > camera_infos_
Definition: camera_aravis_nodelet.h:224
camera_aravis::CameraAravisNodelet::get_string_service_
ros::ServiceServer get_string_service_
Definition: camera_aravis_nodelet.h:177
XmlRpc::XmlRpcValue::TypeDouble
TypeDouble
camera_aravis::CameraBufferPool::Ptr
boost::shared_ptr< CameraBufferPool > Ptr
Definition: camera_buffer_pool.h:91
camera_aravis::CameraAravisNodelet::config_max_
Config config_max_
Definition: camera_aravis_nodelet.h:248
PLUGINLIB_EXPORT_CLASS
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
camera_aravis::CameraAravisNodelet::p_device_
ArvDevice * p_device_
Definition: camera_aravis_nodelet.h:126
ros::Publisher::shutdown
void shutdown()
camera_aravis::CameraAravisNodelet::spawn_stream_thread_
std::thread spawn_stream_thread_
Definition: camera_aravis_nodelet.h:251
camera_aravis::CameraAravisNodelet::config_
Config config_
Definition: camera_aravis_nodelet.h:246
camera_aravis::CameraAravisNodelet::p_camera_info_node_handles_
std::vector< std::unique_ptr< ros::NodeHandle > > p_camera_info_node_handles_
Definition: camera_aravis_nodelet.h:223
tf::getPrefixParam
std::string getPrefixParam(ros::NodeHandle &nh)
camera_aravis::CameraAravisNodelet::setIntegerFeatureCallback
bool setIntegerFeatureCallback(camera_aravis::set_integer_feature_value::Request &request, camera_aravis::set_integer_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:711
camera_aravis::CameraAravisNodelet::reconfigure_server_
std::unique_ptr< dynamic_reconfigure::Server< Config > > reconfigure_server_
Definition: camera_aravis_nodelet.h:218
camera_aravis::CameraAravisNodelet::tuneGvStream
void tuneGvStream(ArvGvStream *p_stream)
Definition: camera_aravis_nodelet.cpp:1082
XmlRpc::XmlRpcValue::TypeString
TypeString
camera_aravis::CameraAravisNodelet::onShutdownTriggered
void onShutdownTriggered(const ros::TimerEvent &)
Definition: camera_aravis_nodelet.cpp:1947
camera_aravis::CameraAravisNodelet::set_boolean_service_
ros::ServiceServer set_boolean_service_
Definition: camera_aravis_nodelet.h:192
camera_aravis::CameraAravisNodelet::writeCameraFeaturesFromRosparam
void writeCameraFeaturesFromRosparam()
Definition: camera_aravis_nodelet.cpp:1878
camera_aravis::CameraAravisNodelet::diagnostic_features_
YAML::Node diagnostic_features_
Definition: camera_aravis_nodelet.h:234
camera_aravis::CameraAravisNodelet::newBufferReadyCallback
static void newBufferReadyCallback(ArvStream *p_stream, gpointer can_instance)
Definition: camera_aravis_nodelet.cpp:1363
camera_info_manager::CameraInfoManager
camera_aravis::CameraAravisNodelet::stream_names_
std::vector< std::string > stream_names_
Definition: camera_aravis_nodelet.h:130
image_transport::CameraPublisher
ROS_WARN
#define ROS_WARN(...)
camera_aravis::CameraAravisNodelet::p_camera_info_managers_
std::vector< std::unique_ptr< camera_info_manager::CameraInfoManager > > p_camera_info_managers_
Definition: camera_aravis_nodelet.h:222
camera_aravis::CameraAravisNodelet::set_float_service_
ros::ServiceServer set_float_service_
Definition: camera_aravis_nodelet.h:186
camera_aravis::CameraAravisNodelet::discoverFeatures
void discoverFeatures()
Definition: camera_aravis_nodelet.cpp:1781
camera_aravis_nodelet.h
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
ros::TimerEvent
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
camera_aravis::CameraAravisNodelet::set_string_service_
ros::ServiceServer set_string_service_
Definition: camera_aravis_nodelet.h:189
ros::names::remap
ROSCPP_DECL std::string remap(const std::string &name)
camera_aravis::CameraAravisNodelet::spawning_
std::atomic< bool > spawning_
Definition: camera_aravis_nodelet.h:250
camera_aravis::CameraAravisNodelet::cam_pubs_
std::vector< image_transport::CameraPublisher > cam_pubs_
Definition: camera_aravis_nodelet.h:221
XmlRpc::XmlRpcValue::iterator
ValueStruct::iterator iterator
camera_aravis::CameraAravisNodelet::roi_
struct camera_aravis::CameraAravisNodelet::@0 roi_
XmlRpc::XmlRpcValue::end
iterator end()
camera_aravis::CameraAravisNodelet::setBooleanFeatureCallback
bool setBooleanFeatureCallback(camera_aravis::set_boolean_feature_value::Request &request, camera_aravis::set_boolean_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:780
ros::Rate::sleep
bool sleep()
XmlRpc::XmlRpcValue::getType
const Type & getType() const
image_transport::SingleSubscriberPublisher
camera_aravis::CameraAravisNodelet::auto_params_
CameraAutoInfo auto_params_
Definition: camera_aravis_nodelet.h:239
XmlRpc::XmlRpcValue::TypeArray
TypeArray
camera_aravis::CameraAravisNodelet::rosReconfigureCallback
void rosReconfigureCallback(Config &config, uint32_t level)
Definition: camera_aravis_nodelet.cpp:1109
camera_aravis::CameraAravisNodelet::pub_ext_camera_info_
bool pub_ext_camera_info_
Definition: camera_aravis_nodelet.h:122
camera_aravis::CameraAravisNodelet::verbose_
bool verbose_
Definition: camera_aravis_nodelet.h:114
camera_aravis::CameraAravisNodelet::StreamIdData
Definition: camera_aravis_nodelet.h:284
camera_aravis::CameraAravisNodelet::parseStringArgs
static void parseStringArgs(std::string in_arg_string, std::vector< std::string > &out_args)
Definition: camera_aravis_nodelet.cpp:1845
nodelet::Nodelet
camera_aravis::CameraAravisNodelet::auto_pub_
ros::Publisher auto_pub_
Definition: camera_aravis_nodelet.h:240
camera_aravis::CameraAravisNodelet::onInit
virtual void onInit() override
Definition: camera_aravis_nodelet.cpp:124
camera_aravis::CameraAravisNodelet::reconfigure_mutex_
boost::recursive_mutex reconfigure_mutex_
Definition: camera_aravis_nodelet.h:219
camera_aravis::CameraAravisNodelet::diagnostic_thread_active_
std::atomic_bool diagnostic_thread_active_
Definition: camera_aravis_nodelet.h:237
camera_aravis::CameraAravisNodelet::setFloatFeatureCallback
bool setFloatFeatureCallback(camera_aravis::set_float_feature_value::Request &request, camera_aravis::set_float_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:734
camera_aravis::CameraAravisNodelet::setAutoMaster
void setAutoMaster(bool value)
Definition: camera_aravis_nodelet.cpp:1010
ROS_ERROR
#define ROS_ERROR(...)
camera_aravis::CameraAravisNodelet::num_streams_
gint num_streams_
Definition: camera_aravis_nodelet.h:128
tf2_ros::TransformBroadcaster
camera_aravis::CameraAravisNodelet::softwareTriggerLoop
void softwareTriggerLoop()
Definition: camera_aravis_nodelet.cpp:1576
camera_aravis::CameraAravisNodelet::controlLostCallback
static void controlLostCallback(ArvDevice *p_gv_device, gpointer can_instance)
Definition: camera_aravis_nodelet.cpp:1566
camera_aravis::CameraAravisNodelet::get_float_service_
ros::ServiceServer get_float_service_
Definition: camera_aravis_nodelet.h:174
nodelet::Nodelet::getName
const std::string & getName() const
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
camera_aravis::CameraAravisNodelet::software_trigger_active_
std::atomic_bool software_trigger_active_
Definition: camera_aravis_nodelet.h:254
camera_aravis::CameraAravisNodelet::ptp_set_cmd_feature_
std::string ptp_set_cmd_feature_
Definition: camera_aravis_nodelet.h:120
image_transport::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::Rate
camera_aravis::CameraAravisNodelet::getFloatFeatureCallback
bool getFloatFeatureCallback(camera_aravis::get_float_feature_value::Request &request, camera_aravis::get_float_feature_value::Response &response)
Definition: camera_aravis_nodelet.cpp:727
ros::Duration::sleep
bool sleep() const
camera_aravis::CameraAravisNodelet::p_stb_
std::unique_ptr< tf2_ros::StaticTransformBroadcaster > p_stb_
Definition: camera_aravis_nodelet.h:226
camera_aravis::CameraAravisNodelet::p_buffer_pools_
std::vector< CameraBufferPool::Ptr > p_buffer_pools_
Definition: camera_aravis_nodelet.h:131
XmlRpc::XmlRpcValue::begin
iterator begin()
XmlRpc::XmlRpcValue::TypeBoolean
TypeBoolean
camera_aravis::CameraAravisNodelet::implemented_features_
std::unordered_map< std::string, const bool > implemented_features_
Definition: camera_aravis_nodelet.h:257
XmlRpc::XmlRpcValue::TypeDateTime
TypeDateTime
camera_aravis::CameraAravisNodelet::resetPtpClock
void resetPtpClock()
Definition: camera_aravis_nodelet.cpp:796
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
camera_aravis::CameraAravisNodelet::setAutoSlave
void setAutoSlave(bool value)
Definition: camera_aravis_nodelet.cpp:1023
ros::Duration
camera_aravis::CameraAravisNodelet::height
int32_t height
Definition: camera_aravis_nodelet.h:269
camera_aravis::CONVERSIONS_DICTIONARY
const std::map< std::string, ConversionFunction > CONVERSIONS_DICTIONARY
Definition: conversion_utils.h:76
camera_aravis::CameraAravisNodelet::use_ptp_stamp_
bool use_ptp_stamp_
Definition: camera_aravis_nodelet.h:117
XmlRpc::XmlRpcValue
ros::NodeHandle
camera_aravis::CameraAravisNodelet::readAndPublishCameraDiagnostics
void readAndPublishCameraDiagnostics(double rate) const
Definition: camera_aravis_nodelet.cpp:1623
ros::Time::now
static Time now()
camera_aravis::CameraAravisNodelet::cameraAutoInfoCallback
void cameraAutoInfoCallback(const CameraAutoInfoConstPtr &msg_ptr)
Definition: camera_aravis_nodelet.cpp:832

camera_aravis
Author(s): Boitumelo Ruf, Fraunhofer IOSB , Dominik Kleiser, Fraunhofer IOSB , Dominik A. Klein, Fraunhofer FKIE , Steve Safarik, Straw Lab , Andrew Straw, Straw Lab , Floris van Breugel, van Breugel Lab
autogenerated on Thu Jul 25 2024 02:25:15