42 arv_stream_set_emit_signals(
p_streams_[i], FALSE);
72 guint64 n_completed_buffers = 0;
73 guint64 n_failures = 0;
74 guint64 n_underruns = 0;
75 arv_stream_get_statistics(
p_streams_[i], &n_completed_buffers, &n_failures, &n_underruns);
76 ROS_INFO(
"Completed buffers = %Lu", (
unsigned long long ) n_completed_buffers);
77 ROS_INFO(
"Failures = %Lu", (
unsigned long long ) n_failures);
78 ROS_INFO(
"Underruns = %Lu", (
unsigned long long ) n_underruns);
83 arv_gv_stream_get_statistics(
reinterpret_cast<ArvGvStream*
>(
p_streams_[i]), &n_resent, &n_missing);
84 ROS_INFO(
"Resent buffers = %Lu", (
unsigned long long ) n_resent);
85 ROS_INFO(
"Missing = %Lu", (
unsigned long long ) n_missing);
92 arv_device_execute_command(
p_device_,
"AcquisitionStop");
115 bool init_params_from_reconfig = pnh.
param<
bool>(
"init_params_from_dyn_reconfigure",
true);
117 std::string awb_mode = pnh.
param<std::string>(
"BalanceWhiteAuto",
"Continuous");
118 std::string wb_ratio_selector_feature = pnh.
param<std::string>(
"wb_ratio_selector_feature",
"");
119 std::string wb_ratio_feature = pnh.
param<std::string>(
"wb_ratio_feature",
"");
121 std::string wb_ratio_selector_args = pnh.
param<std::string>(
"wb_ratio_selectors",
"");
122 std::vector<std::string> wb_ratio_selector_values;
125 std::string wb_ratio_args = pnh.
param<std::string>(
"wb_ratios",
"");
126 std::vector<std::string> wb_ratio_str_values;
129 if(wb_ratio_selector_values.size() != wb_ratio_str_values.size())
131 ROS_WARN(
"Lists of 'wb_ratio_selector' and 'wb_ratio' have different sizes. "
132 "Only setting values which exist in both lists.");
138 std::string stream_channel_args;
139 if (pnh.
getParam(
"channel_names", stream_channel_args)) {
145 std::string pixel_format_args;
146 std::vector<std::string> pixel_formats;
147 pnh.
param(
"PixelFormat", pixel_format_args, pixel_format_args);
150 std::string calib_url_args;
151 std::vector<std::string> calib_urls;
152 pnh.
param(
"camera_info_url", calib_url_args, calib_url_args);
157 std::max(0.0, pnh.
param<
double>(
"diagnostic_publish_rate", 0.1));
165 arv_update_device_list();
166 uint n_interfaces = arv_get_n_interfaces();
167 ROS_INFO(
"# Interfaces: %d", n_interfaces);
169 uint n_devices = arv_get_n_devices();
170 ROS_INFO(
"# Devices: %d", n_devices);
171 for (uint i = 0; i < n_devices; i++)
172 ROS_INFO(
"Device%d: %s", i, arv_get_device_id(i));
197 const char* vendor_name = arv_camera_get_vendor_name(
p_camera_);
198 const char* model_name = arv_camera_get_model_name(
p_camera_);
199 const char* device_id = arv_camera_get_device_id(
p_camera_);
200 const char* device_sn = arv_device_get_string_feature_value(
p_device_,
"DeviceSerialNumber");
201 ROS_INFO(
"Successfully Opened: %s-%s-%s", vendor_name, model_name,
202 (device_sn) ? device_sn : device_id);
221 ROS_WARN(
"Unable to detect number of supported stream channels.");
248 arv_camera_gv_select_stream_channel(
p_camera_,i);
256 gint64 focus_min64, focus_max64;
257 arv_device_get_integer_feature_bounds(
p_device_,
"FocusPos", &focus_min64, &focus_max64);
268 arv_camera_gv_select_stream_channel(
p_camera_,i);
270 if(init_params_from_reconfig)
276 arv_device_set_float_feature_value(
p_device_,
"ExposureTimeAbs",
config_.ExposureTime);
280 arv_device_set_integer_feature_value(
p_device_,
"AcquisitionFrameRateEnable", 1);
290 arv_device_set_string_feature_value(
p_device_,
"TriggerSelector",
"FrameStart");
291 arv_device_set_string_feature_value(
p_device_,
"TriggerMode",
"Off");
302 for(
int l = 0; l < std::min(wb_ratio_selector_values.size(), wb_ratio_str_values.size());
305 arv_device_set_string_feature_value(
p_device_, wb_ratio_selector_feature.c_str(),
306 wb_ratio_selector_values[l].c_str());
307 arv_device_set_float_feature_value(
p_device_, wb_ratio_feature.c_str(),
308 std::stof(wb_ratio_str_values[l]));
336 arv_camera_gv_select_stream_channel(
p_camera_,i);
337 std::string source_selector =
"Source" + std::to_string(i);
338 arv_device_set_string_feature_value(
p_device_,
"SourceSelector", source_selector.c_str());
339 arv_device_set_string_feature_value(
p_device_,
"PixelFormat", pixel_formats[i].c_str());
340 sensors_[i]->pixel_format = std::string(arv_device_get_string_feature_value(
p_device_,
"PixelFormat"));
346 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.");
349 sensors_[i]->n_bits_pixel = ARV_PIXEL_FORMAT_BIT_PER_PIXEL(
350 arv_device_get_integer_feature_value(
p_device_,
"PixelFormat"));
357 pnh.
param<
double>(
"softwaretriggerrate",
config_.softwaretriggerrate,
config_.softwaretriggerrate);
377 double tf_publish_rate;
378 pnh.
param<
double>(
"tf_publish_rate", tf_publish_rate, 0);
379 if (tf_publish_rate > 0.)
401 catch(
const YAML::BadFile& e)
403 ROS_WARN(
"YAML file cannot be loaded: %s", e.what());
404 ROS_WARN(
"Camera diagnostics will not be published.");
406 catch(
const YAML::ParserException& e)
408 ROS_WARN(
"YAML file is malformed: %s", e.what());
409 ROS_WARN(
"Camera diagnostics will not be published.");
423 ROS_WARN(
"No diagnostic features specified.");
424 ROS_WARN(
"Camera diagnostics will not be published.");
429 ROS_WARN(
"diagnostic_publish_rate is <= 0.0");
430 ROS_WARN(
"Camera diagnostics will not be published.");
434 ArvGcNode *p_gc_node;
435 GError *error = NULL;
437 p_gc_node = arv_device_get_feature(
p_device_,
"DeviceSerialNumber");
439 if(calib_urls[0].empty()) {
440 if( arv_gc_feature_node_is_implemented( ARV_GC_FEATURE_NODE(p_gc_node), &error) ) {
441 GType device_serial_return_type = arv_gc_feature_node_get_value_type( ARV_GC_FEATURE_NODE(p_gc_node));
445 if (device_serial_return_type == G_TYPE_STRING) {
446 calib_urls[0] = arv_device_get_string_feature_value(
p_device_,
"DeviceSerialNumber");
447 calib_urls[0] +=
".yaml";
448 }
else if (device_serial_return_type == G_TYPE_INT64) {
449 calib_urls[0] = arv_device_get_string_feature_value(
p_device_,
"DeviceID");
450 calib_urls[0] +=
".yaml";
457 std::string camera_info_frame_id =
config_.frame_id;
485 ROS_INFO(
" Using Camera Configuration:");
486 ROS_INFO(
" ---------------------------");
487 ROS_INFO(
" Vendor name = %s", arv_device_get_string_feature_value(
p_device_,
"DeviceVendorName"));
488 ROS_INFO(
" Model name = %s", arv_device_get_string_feature_value(
p_device_,
"DeviceModelName"));
489 ROS_INFO(
" Device id = %s", arv_device_get_string_feature_value(
p_device_,
"DeviceUserID"));
490 ROS_INFO(
" Serial number = %s", arv_device_get_string_feature_value(
p_device_,
"DeviceSerialNumber"));
493 arv_camera_is_uv_device(
p_camera_) ?
"USB3Vision" :
494 (arv_camera_is_gv_device(
p_camera_) ?
"GigEVision" :
"Other"));
497 ROS_INFO(
" ROI x,y,w,h = %d, %d, %d, %d",
roi_.x,
roi_.y,
roi_.width,
roi_.height);
502 "(not implemented in camera)");
505 "(not implemented in camera)");
508 "(not implemented in camera)");
512 ROS_INFO(
" AcquisitionFrameRate = %g hz",
config_.AcquisitionFrameRate);
517 ROS_INFO(
" BalanceWhiteAuto = %s", awb_mode.c_str());
518 if(awb_mode !=
"Continuous"
522 for(
int l = 0; l < std::min(wb_ratio_selector_values.size(), wb_ratio_str_values.size());
525 arv_device_set_string_feature_value(
p_device_, wb_ratio_selector_feature.c_str(),
526 wb_ratio_selector_values[l].c_str());
527 float wb_ratio_val = arv_device_get_float_feature_value(
p_device_, wb_ratio_feature.c_str());
528 ROS_INFO(
" BalanceRatio %s = %f", wb_ratio_selector_values[l].c_str(), wb_ratio_val);
551 ROS_INFO(
" Network mtu = %lu", arv_device_get_integer_feature_value(
p_device_,
"GevSCPSPacketSize"));
553 ROS_INFO(
" ---------------------------");
567 ROS_WARN(
"PTP Error: ptp_set_cmd_feature_ '%s' is not available on the device.",
584 arv_camera_gv_select_stream_channel(
p_camera_, i);
590 arv_camera_gv_select_stream_channel(
p_camera_, i);
591 const gint n_bytes_payload_stream_ = arv_camera_get_payload(
p_camera_);
603 ROS_WARN(
"Stream %i: Could not create image stream for %s. Retrying...", i,
guid_.c_str());
611 std::vector<image_transport::SubscriberStatusCallback> image_cbs_;
612 std::vector<ros::SubscriberStatusCallback> info_cbs_;
622 std::string topic_name = this->
getName();
630 1, image_cb, image_cb, info_cb, info_cb);
635 StreamIdData* data =
new StreamIdData();
643 arv_stream_set_emit_signals(
p_streams_[i], TRUE);
662 ROS_INFO(
"Done initializing camera_aravis.");
667 const char* feature_name = request.feature.c_str();
668 response.response = arv_device_get_integer_feature_value(this->
p_device_, feature_name);
674 const char* feature_name = request.feature.c_str();
675 guint64 value = request.value;
676 arv_device_set_integer_feature_value(this->
p_device_, feature_name, value);
677 if(arv_device_get_status(this->
p_device_) == ARV_DEVICE_STATUS_SUCCESS) {
687 const char* feature_name = request.feature.c_str();
688 response.response = arv_device_get_float_feature_value(this->
p_device_, feature_name);
694 const char* feature_name = request.feature.c_str();
695 const double value = request.value;
696 arv_device_set_float_feature_value(this->
p_device_, feature_name, value);
697 if(arv_device_get_status(this->
p_device_) == ARV_DEVICE_STATUS_SUCCESS) {
707 const char* feature_name = request.feature.c_str();
708 response.response = arv_device_get_string_feature_value(this->
p_device_, feature_name);
714 const char* feature_name = request.feature.c_str();
715 const char* value = request.value.c_str();
716 arv_device_set_string_feature_value(this->
p_device_, feature_name, value);
717 if(arv_device_get_status(this->
p_device_) == ARV_DEVICE_STATUS_SUCCESS) {
727 const char* feature_name = request.feature.c_str();
728 response.response = arv_device_get_boolean_feature_value(this->
p_device_, feature_name);
734 const char* feature_name = request.feature.c_str();
735 const bool value = request.value;
736 arv_device_set_boolean_feature_value(this->
p_device_, feature_name, value);
737 if(arv_device_get_status(this->
p_device_) == ARV_DEVICE_STATUS_SUCCESS) {
749 ROS_WARN(
"PTP Error: ptp_status_feature_name and/or ptp_enable_feature_name are empty.");
756 ROS_WARN(
"PTP Error: ptp_status_feature_name '%s' is not available on the device.",
759 ROS_WARN(
"PTP Error: ptp_enable_feature_name '%s' is not available on the device.",
765 std::string ptp_status_str =
768 if (ptp_status_str ==
"Faulty" ||
769 ptp_status_str ==
"Disabled" ||
770 ptp_status_str ==
"Initializing" ||
773 ROS_INFO(
"Resetting ptp clock (was set to %s)", ptp_status_str.c_str());
787 arv_device_set_float_feature_value(
p_device_,
"ExposureTime", msg_ptr->exposure_time);
796 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"All");
798 arv_device_set_float_feature_value(
p_device_,
"Gain", msg_ptr->gain);
805 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"Red");
806 arv_device_set_float_feature_value(
p_device_,
"Gain", msg_ptr->gain_red);
811 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"Green");
812 arv_device_set_float_feature_value(
p_device_,
"Gain", msg_ptr->gain_green);
817 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"Blue");
818 arv_device_set_float_feature_value(
p_device_,
"Gain", msg_ptr->gain_blue);
829 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"All");
831 arv_device_set_float_feature_value(
p_device_,
"BlackLevel", msg_ptr->black_level);
838 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"Red");
839 arv_device_set_float_feature_value(
p_device_,
"BlackLevel", msg_ptr->bl_red);
844 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"Green");
845 arv_device_set_float_feature_value(
p_device_,
"BlackLevel", msg_ptr->bl_green);
850 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"Blue");
851 arv_device_set_float_feature_value(
p_device_,
"BlackLevel", msg_ptr->bl_blue);
857 if (strcmp(
"The Imaging Source Europe GmbH", arv_camera_get_vendor_name(
p_camera_)) == 0)
859 arv_device_set_integer_feature_value(
p_device_,
"WhiteBalanceRedRegister", (
int)(
auto_params_.wb_red * 255.));
860 arv_device_set_integer_feature_value(
p_device_,
"WhiteBalanceGreenRegister", (
int)(
auto_params_.wb_green * 255.));
861 arv_device_set_integer_feature_value(
p_device_,
"WhiteBalanceBlueRegister", (
int)(
auto_params_.wb_blue * 255.));
867 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Red");
868 arv_device_set_float_feature_value(
p_device_,
"BalanceRatio", msg_ptr->wb_red);
873 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Green");
874 arv_device_set_float_feature_value(
p_device_,
"BalanceRatio", msg_ptr->wb_green);
879 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Blue");
880 arv_device_set_float_feature_value(
p_device_,
"BalanceRatio", msg_ptr->wb_blue);
893 std::numeric_limits<double>::quiet_NaN();
906 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"All");
911 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"Red");
913 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"Green");
915 arv_device_set_string_feature_value(
p_device_,
"GainSelector",
"Blue");
924 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"All");
929 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"Red");
931 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"Green");
933 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"Blue");
939 if (strcmp(
"The Imaging Source Europe GmbH", arv_camera_get_vendor_name(
p_camera_)) == 0)
941 auto_params_.wb_red = arv_device_get_integer_feature_value(
p_device_,
"WhiteBalanceRedRegister") / 255.;
942 auto_params_.wb_green = arv_device_get_integer_feature_value(
p_device_,
"WhiteBalanceGreenRegister") / 255.;
943 auto_params_.wb_blue = arv_device_get_integer_feature_value(
p_device_,
"WhiteBalanceBlueRegister") / 255.;
948 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Red");
950 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Green");
952 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Blue");
978 arv_device_set_string_feature_value(
p_device_,
"ExposureAuto",
"Off");
982 arv_device_set_string_feature_value(
p_device_,
"GainAuto",
"Off");
986 arv_device_set_string_feature_value(
p_device_,
"GainAutoBalance",
"Off");
990 arv_device_set_string_feature_value(
p_device_,
"BlackLevelAuto",
"Off");
994 arv_device_set_string_feature_value(
p_device_,
"BlackLevelAutoBalance",
"Off");
998 arv_device_set_string_feature_value(
p_device_,
"BalanceWhiteAuto",
"Off");
1014 if (channel_name.empty()) {
1029 gboolean b_auto_buffer = FALSE;
1030 gboolean b_packet_resend = TRUE;
1031 unsigned int timeout_packet = 40;
1032 unsigned int timeout_frame_retention = 200;
1036 if (!ARV_IS_GV_STREAM(p_stream))
1038 ROS_WARN(
"Stream is not a GV_STREAM");
1043 g_object_set(p_stream,
"socket-buffer", ARV_GV_STREAM_SOCKET_BUFFER_AUTO,
"socket-buffer-size", 0,
1045 if (!b_packet_resend)
1046 g_object_set(p_stream,
"packet-resend",
1047 b_packet_resend ? ARV_GV_STREAM_PACKET_RESEND_ALWAYS : ARV_GV_STREAM_PACKET_RESEND_NEVER,
1049 g_object_set(p_stream,
"packet-timeout", timeout_packet * 1000,
"frame-retention", timeout_frame_retention * 1000,
1060 if (config.frame_id ==
"")
1061 config.frame_id = this->
getName();
1064 config.AcquisitionFrameRate = CLAMP(config.AcquisitionFrameRate,
config_min_.AcquisitionFrameRate,
1069 config.frame_id =
tf::resolve(tf_prefix, config.frame_id);
1075 if (config.AutoSlave)
1077 config.ExposureAuto =
"Off";
1078 config.GainAuto =
"Off";
1083 if (config.ExposureAuto.compare(
"Off") != 0)
1085 config.ExposureTime =
config_.ExposureTime;
1086 ROS_WARN(
"ExposureAuto is active. Cannot manually set ExposureTime.");
1088 if (config.GainAuto.compare(
"Off") != 0)
1091 ROS_WARN(
"GainAuto is active. Cannot manually set Gain.");
1095 if (config.TriggerMode.compare(
"Off") != 0)
1097 config.AcquisitionFrameRate =
config_.AcquisitionFrameRate;
1098 ROS_WARN(
"TriggerMode is active (Trigger Source: %s). Cannot manually set AcquisitionFrameRate.",
config_.TriggerSource.c_str());
1102 const bool changed_auto_master = (
config_.AutoMaster != config.AutoMaster);
1103 const bool changed_auto_slave = (
config_.AutoSlave != config.AutoSlave);
1104 const bool changed_acquisition_frame_rate = (
config_.AcquisitionFrameRate != config.AcquisitionFrameRate);
1105 const bool changed_exposure_auto = (
config_.ExposureAuto != config.ExposureAuto);
1106 const bool changed_exposure_time = (
config_.ExposureTime != config.ExposureTime);
1107 const bool changed_gain_auto = (
config_.GainAuto != config.GainAuto);
1108 const bool changed_gain = (
config_.Gain != config.Gain);
1109 const bool changed_acquisition_mode = (
config_.AcquisitionMode != config.AcquisitionMode);
1110 const bool changed_trigger_mode = (
config_.TriggerMode != config.TriggerMode);
1111 const bool changed_trigger_source = (
config_.TriggerSource != config.TriggerSource) || changed_trigger_mode;
1112 const bool changed_focus_pos = (
config_.FocusPos != config.FocusPos);
1113 const bool changed_mtu = (
config_.mtu != config.mtu);
1115 if (changed_auto_master)
1120 if (changed_auto_slave)
1126 if (changed_exposure_time)
1130 ROS_INFO(
"Set ExposureTime = %f us", config.ExposureTime);
1131 arv_camera_set_exposure_time(
p_camera_, config.ExposureTime);
1134 ROS_INFO(
"Camera does not support ExposureTime.");
1141 ROS_INFO(
"Set gain = %f", config.Gain);
1142 arv_camera_set_gain(
p_camera_, config.Gain);
1145 ROS_INFO(
"Camera does not support Gain or GainRaw.");
1148 if (changed_exposure_auto)
1152 ROS_INFO(
"Set ExposureAuto = %s", config.ExposureAuto.c_str());
1153 arv_device_set_string_feature_value(
p_device_,
"ExposureAuto", config.ExposureAuto.c_str());
1154 if (config.ExposureAuto.compare(
"Once") == 0)
1157 config.ExposureTime = arv_camera_get_exposure_time(
p_camera_);
1158 ROS_INFO(
"Get ExposureTime = %f us", config.ExposureTime);
1159 config.ExposureAuto =
"Off";
1163 ROS_INFO(
"Camera does not support ExposureAuto.");
1165 if (changed_gain_auto)
1169 ROS_INFO(
"Set GainAuto = %s", config.GainAuto.c_str());
1170 arv_device_set_string_feature_value(
p_device_,
"GainAuto", config.GainAuto.c_str());
1171 if (config.GainAuto.compare(
"Once") == 0)
1174 config.Gain = arv_camera_get_gain(
p_camera_);
1175 ROS_INFO(
"Get Gain = %f", config.Gain);
1176 config.GainAuto =
"Off";
1180 ROS_INFO(
"Camera does not support GainAuto.");
1183 if (changed_acquisition_frame_rate)
1187 ROS_INFO(
"Set frame rate = %f Hz", config.AcquisitionFrameRate);
1188 arv_camera_set_frame_rate(
p_camera_, config.AcquisitionFrameRate);
1191 ROS_INFO(
"Camera does not support AcquisitionFrameRate.");
1194 if (changed_trigger_mode)
1198 ROS_INFO(
"Set TriggerMode = %s", config.TriggerMode.c_str());
1199 arv_device_set_string_feature_value(
p_device_,
"TriggerMode", config.TriggerMode.c_str());
1202 ROS_INFO(
"Camera does not support TriggerMode.");
1205 if (changed_trigger_source)
1216 ROS_INFO(
"Set TriggerSource = %s", config.TriggerSource.c_str());
1217 arv_device_set_string_feature_value(
p_device_,
"TriggerSource", config.TriggerSource.c_str());
1221 ROS_INFO(
"Camera does not support TriggerSource.");
1225 if (config.TriggerMode.compare(
"On") == 0 && config.TriggerSource.compare(
"Software") == 0)
1229 config_.softwaretriggerrate = config.softwaretriggerrate;
1230 ROS_INFO(
"Set softwaretriggerrate = %f", 1000.0 / ceil(1000.0 / config.softwaretriggerrate));
1237 ROS_INFO(
"Camera does not support TriggerSoftware command.");
1242 if (changed_focus_pos)
1246 ROS_INFO(
"Set FocusPos = %d", config.FocusPos);
1247 arv_device_set_integer_feature_value(
p_device_,
"FocusPos", config.FocusPos);
1249 config.FocusPos = arv_device_get_integer_feature_value(
p_device_,
"FocusPos");
1250 ROS_INFO(
"Get FocusPos = %d", config.FocusPos);
1253 ROS_INFO(
"Camera does not support FocusPos.");
1260 ROS_INFO(
"Set mtu = %d", config.mtu);
1261 arv_device_set_integer_feature_value(
p_device_,
"GevSCPSPacketSize", config.mtu);
1263 config.mtu = arv_device_get_integer_feature_value(
p_device_,
"GevSCPSPacketSize");
1264 ROS_INFO(
"Get mtu = %d", config.mtu);
1267 ROS_INFO(
"Camera does not support mtu (i.e. GevSCPSPacketSize).");
1270 if (changed_acquisition_mode)
1274 ROS_INFO(
"Set AcquisitionMode = %s", config.AcquisitionMode.c_str());
1275 arv_device_set_string_feature_value(
p_device_,
"AcquisitionMode", config.AcquisitionMode.c_str());
1278 arv_device_execute_command(
p_device_,
"AcquisitionStop");
1280 arv_device_execute_command(
p_device_,
"AcquisitionStart");
1283 ROS_INFO(
"Camera does not support AcquisitionMode.");
1298 arv_device_execute_command(
p_device_,
"AcquisitionStop");
1302 arv_device_execute_command(
p_device_,
"AcquisitionStart");
1311 StreamIdData *data = (StreamIdData*) can_instance;
1313 size_t stream_id = data->stream_id;
1320 const std::string stream_frame_id = p_can->
config_.frame_id +
"/" + p_can->
stream_names_[stream_id];
1328 ArvBuffer *p_buffer = arv_stream_try_pop_buffer(p_stream);
1331 gint n_available_buffers;
1332 arv_stream_get_n_buffers(p_stream, &n_available_buffers, NULL);
1333 if (n_available_buffers == 0)
1335 p_can->p_buffer_pools_[stream_id]->allocateBuffers(1);
1338 if (p_buffer != NULL)
1340 if (arv_buffer_get_status(p_buffer) == ARV_BUFFER_STATUS_SUCCESS && p_can->p_buffer_pools_[stream_id]
1341 && p_can->cam_pubs_[stream_id].getNumSubscribers())
1343 (p_can->n_buffers_)++;
1345 sensor_msgs::ImagePtr msg_ptr = (*(p_can->p_buffer_pools_[stream_id]))[p_buffer];
1349 if (p_can->use_ptp_stamp_)
1350 t = arv_buffer_get_timestamp(p_buffer);
1352 t = arv_buffer_get_system_timestamp(p_buffer);
1353 msg_ptr->header.stamp.fromNSec(t);
1355 msg_ptr->header.seq = arv_buffer_get_frame_id(p_buffer);
1357 msg_ptr->header.frame_id = frame_id;
1358 msg_ptr->width = p_can->roi_.width;
1359 msg_ptr->height = p_can->roi_.height;
1360 msg_ptr->encoding = p_can->sensors_[stream_id]->pixel_format;
1361 msg_ptr->step = (msg_ptr->width * p_can->sensors_[stream_id]->n_bits_pixel)/8;
1364 if (p_can->convert_formats[stream_id]) {
1365 sensor_msgs::ImagePtr cvt_msg_ptr = p_can->p_buffer_pools_[stream_id]->getRecyclableImg();
1366 p_can->convert_formats[stream_id](msg_ptr, cvt_msg_ptr);
1367 msg_ptr = cvt_msg_ptr;
1371 if (!p_can->camera_infos_[stream_id]) {
1372 p_can->camera_infos_[stream_id].reset(
new sensor_msgs::CameraInfo);
1374 (*p_can->camera_infos_[stream_id]) = p_can->p_camera_info_managers_[stream_id]->getCameraInfo();
1375 p_can->camera_infos_[stream_id]->header = msg_ptr->header;
1376 if (p_can->camera_infos_[stream_id]->width == 0 || p_can->camera_infos_[stream_id]->height == 0) {
1378 "The fields image_width and image_height seem not to be set in "
1379 "the YAML specified by 'camera_info_url' parameter. Please set "
1380 "them there, because actual image size and specified image size "
1381 "can be different due to the region of interest (ROI) feature. In "
1382 "the YAML the image size should be the one on which the camera was "
1383 "calibrated. See CameraInfo.msg specification!");
1384 p_can->camera_infos_[stream_id]->width = p_can->roi_.width;
1385 p_can->camera_infos_[stream_id]->height = p_can->roi_.height;
1389 p_can->cam_pubs_[stream_id].publish(msg_ptr, p_can->camera_infos_[stream_id]);
1391 if (p_can->pub_ext_camera_info_) {
1392 ExtendedCameraInfo extended_camera_info_msg;
1393 p_can->extended_camera_info_mutex_.lock();
1394 arv_camera_gv_select_stream_channel(p_can->p_camera_, stream_id);
1395 extended_camera_info_msg.camera_info = *(p_can->camera_infos_[stream_id]);
1396 p_can->fillExtendedCameraInfoMessage(extended_camera_info_msg);
1397 p_can->extended_camera_info_mutex_.unlock();
1398 p_can->extended_camera_info_pubs_[stream_id].publish(extended_camera_info_msg);
1402 if (p_can->use_ptp_stamp_)
1403 p_can->resetPtpClock();
1407 if (arv_buffer_get_status(p_buffer) != ARV_BUFFER_STATUS_SUCCESS) {
1410 arv_stream_push_buffer(p_stream, p_buffer);
1415 if (p_can->config_.AutoMaster)
1417 p_can->syncAutoParameters();
1418 p_can->auto_pub_.publish(p_can->auto_params_);
1424 const char *vendor_name = arv_camera_get_vendor_name(
p_camera_);
1426 if (strcmp(
"Basler", vendor_name) == 0) {
1427 msg.exposure_time = arv_device_get_float_feature_value(
p_device_,
"ExposureTimeAbs");
1431 msg.exposure_time = arv_device_get_float_feature_value(
p_device_,
"ExposureTime");
1434 if (strcmp(
"Basler", vendor_name) == 0) {
1435 msg.gain =
static_cast<float>(arv_device_get_integer_feature_value(
p_device_,
"GainRaw"));
1439 msg.gain = arv_device_get_float_feature_value(
p_device_,
"Gain");
1441 if (strcmp(
"Basler", vendor_name) == 0) {
1442 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"All");
1443 msg.black_level =
static_cast<float>(arv_device_get_integer_feature_value(
p_device_,
"BlackLevelRaw"));
1444 }
else if (strcmp(
"JAI Corporation", vendor_name) == 0) {
1447 msg.black_level = 0;
1449 arv_device_set_string_feature_value(
p_device_,
"BlackLevelSelector",
"All");
1450 msg.black_level = arv_device_get_float_feature_value(
p_device_,
"BlackLevel");
1454 if (strcmp(
"The Imaging Source Europe GmbH", vendor_name) == 0)
1456 msg.white_balance_red = arv_device_get_integer_feature_value(
p_device_,
"WhiteBalanceRedRegister") / 255.;
1457 msg.white_balance_green = arv_device_get_integer_feature_value(
p_device_,
"WhiteBalanceGreenRegister") / 255.;
1458 msg.white_balance_blue = arv_device_get_integer_feature_value(
p_device_,
"WhiteBalanceBlueRegister") / 255.;
1462 else if (strcmp(
"JAI Corporation", vendor_name) == 0)
1464 msg.white_balance_red = 1.0;
1465 msg.white_balance_green = 1.0;
1466 msg.white_balance_blue = 1.0;
1469 else if (strcmp(
"Basler", vendor_name) == 0)
1471 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Red");
1472 msg.white_balance_red = arv_device_get_float_feature_value(
p_device_,
"BalanceRatioAbs");
1473 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Green");
1474 msg.white_balance_green = arv_device_get_float_feature_value(
p_device_,
"BalanceRatioAbs");
1475 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Blue");
1476 msg.white_balance_blue = arv_device_get_float_feature_value(
p_device_,
"BalanceRatioAbs");
1481 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Red");
1482 msg.white_balance_red = arv_device_get_float_feature_value(
p_device_,
"BalanceRatio");
1483 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Green");
1484 msg.white_balance_green = arv_device_get_float_feature_value(
p_device_,
"BalanceRatio");
1485 arv_device_set_string_feature_value(
p_device_,
"BalanceRatioSelector",
"Blue");
1486 msg.white_balance_blue = arv_device_get_float_feature_value(
p_device_,
"BalanceRatio");
1489 if (strcmp(
"Basler", vendor_name) == 0) {
1490 msg.temperature =
static_cast<float>(arv_device_get_float_feature_value(
p_device_,
"TemperatureAbs"));
1494 msg.temperature = arv_device_get_float_feature_value(
p_device_,
"DeviceTemperature");
1502 ROS_ERROR(
"Control to aravis device lost.\n\t> Nodelet name: %s."
1503 "\n\t> Shutting down. Allowing for respawn.",
1504 p_can->getName().c_str());
1506 p_can->shutdown_trigger_.start();
1512 ROS_INFO(
"Software trigger started.");
1513 std::chrono::system_clock::time_point next_time = std::chrono::system_clock::now();
1516 next_time += std::chrono::milliseconds(
size_t(std::round(1000.0 /
config_.softwaretriggerrate)));
1521 arv_device_execute_command(
p_device_,
"TriggerSoftware");
1523 if (next_time > std::chrono::system_clock::now())
1525 std::this_thread::sleep_until(next_time);
1529 ROS_WARN(
"Camera Aravis: Missed a software trigger event.");
1530 next_time = std::chrono::system_clock::now();
1533 ROS_INFO(
"Software trigger stopped.");
1539 ROS_WARN(
"Publishing dynamic camera transforms (/tf) at %g Hz", rate);
1558 ROS_INFO(
"Publishing camera diagnostics at %g Hz", rate);
1562 CameraDiagnostics camDiagnosticMsg;
1563 camDiagnosticMsg.header.frame_id =
config_.frame_id;
1566 auto getFeatureValueAsStrFn = [&](
const std::string &name,
const std::string &type)
1569 std::string valueStr =
"";
1572 valueStr = std::to_string(arv_device_get_float_feature_value(
p_device_, name.c_str()));
1573 else if (type ==
"int")
1574 valueStr = std::to_string(arv_device_get_integer_feature_value(
p_device_, name.c_str()));
1575 else if (type ==
"bool")
1576 valueStr = arv_device_get_boolean_feature_value(
p_device_, name.c_str()) ?
"true" :
"false";
1578 valueStr = arv_device_get_string_feature_value(
p_device_, name.c_str());
1584 auto setFeatureValueFromStrFn = [&](
const std::string &name,
const std::string &type,
1585 const std::string &valueStr)
1588 arv_device_set_float_feature_value(
p_device_, name.c_str(), std::stod(valueStr));
1589 else if (type ==
"int")
1590 arv_device_set_integer_feature_value(
p_device_, name.c_str(), std::stoi(valueStr));
1591 else if (type ==
"bool")
1592 arv_device_set_boolean_feature_value(
p_device_, name.c_str(),
1593 (valueStr ==
"true" || valueStr ==
"True" || valueStr ==
"TRUE"));
1595 arv_device_set_string_feature_value(
p_device_, name.c_str(), valueStr.c_str());
1601 ++camDiagnosticMsg.header.seq;
1603 camDiagnosticMsg.data.clear();
1607 std::string featureName = featureDict[
"FeatureName"].IsDefined()
1608 ? featureDict[
"FeatureName"].as<std::string>()
1610 std::string featureType = featureDict[
"Type"].IsDefined()
1611 ? featureDict[
"Type"].as<std::string>()
1614 if(featureName.empty() || featureType.empty())
1617 "Diagnostic feature at index %i does not have a field 'FeatureName' or 'Type'.",
1624 std::transform(featureType.begin(), featureType.end(), featureType.begin(),
1625 [](
unsigned char c){ return std::tolower(c); });
1631 diagnostic_msgs::KeyValue kvPair;
1634 if(featureDict[
"Selectors"].IsDefined() && featureDict[
"Selectors"].size() > 0)
1636 int selectorIdx = 1;
1637 for(
auto selectorDict : featureDict[
"Selectors"])
1639 std::string selectorFeatureName = selectorDict[
"FeatureName"].IsDefined()
1640 ? selectorDict[
"FeatureName"].as<std::string>()
1642 std::string selectorType = selectorDict[
"Type"].IsDefined()
1643 ? selectorDict[
"Type"].as<std::string>()
1645 std::string selectorValue = selectorDict[
"Value"].IsDefined()
1646 ? selectorDict[
"Value"].as<std::string>()
1649 if(selectorFeatureName.empty() || selectorType.empty() || selectorValue.empty())
1652 "Diagnostic feature selector at index %i of feature at index %i does not have a "
1653 "field 'FeatureName', 'Type' or 'Value'.",
1654 selectorIdx, featureIdx);
1662 setFeatureValueFromStrFn(selectorFeatureName, selectorType, selectorValue);
1664 kvPair.key = featureName +
"-" + selectorValue;
1665 kvPair.value = getFeatureValueAsStrFn(featureName, featureType);
1667 camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair));
1671 ROS_WARN_ONCE(
"Diagnostic feature selector with name '%s' is not implemented.",
1672 selectorFeatureName.c_str());
1681 kvPair.key = featureName;
1682 kvPair.value = getFeatureValueAsStrFn(featureName, featureType);
1684 camDiagnosticMsg.data.push_back(diagnostic_msgs::KeyValue(kvPair));
1689 ROS_WARN_ONCE(
"Diagnostic feature with name '%s' is not implemented.",
1690 featureName.c_str());
1711 ArvGc *gc = arv_device_get_genicam(
p_device_);
1715 std::list<ArvDomNode*> todo;
1716 todo.push_front((ArvDomNode*)arv_gc_get_node(gc,
"Root"));
1718 while (!todo.empty())
1721 ArvDomNode *node = todo.front();
1723 const std::string name(arv_dom_node_get_node_name(node));
1728 if (name.compare(
"pInvalidator") == 0)
1732 ArvDomNode *inode = (ArvDomNode*)arv_gc_get_node(gc,
1733 arv_dom_node_get_node_value(arv_dom_node_get_first_child(node)));
1735 todo.push_front(inode);
1740 if (ARV_IS_GC_FEATURE_NODE(node))
1743 ArvGcFeatureNode *fnode = ARV_GC_FEATURE_NODE(node);
1744 const std::string fname(arv_gc_feature_node_get_name(fnode));
1745 const bool usable = arv_gc_feature_node_is_available(fnode, NULL)
1746 && arv_gc_feature_node_is_implemented(fnode, NULL);
1760 ArvDomNodeList *children = arv_dom_node_get_child_nodes(node);
1761 const uint l = arv_dom_node_list_get_length(children);
1762 for (uint i = 0; i < l; ++i)
1764 todo.push_front(arv_dom_node_list_get_item(children, i));
1770 size_t array_start = 0;
1771 size_t array_end = in_arg_string.length();
1772 if(array_start != std::string::npos && array_end != std::string::npos) {
1774 std::stringstream ss(in_arg_string.substr(array_start, array_end - array_start));
1777 getline( ss, temp,
',');
1778 boost::trim_left(temp);
1779 boost::trim_right(temp);
1780 out_args.push_back(temp);
1784 out_args.push_back(in_arg_string);
1801 ArvGcNode *p_gc_node;
1802 GError *error = NULL;
1808 for (iter = xml_rpc_params.
begin(); iter != xml_rpc_params.
end(); iter++)
1810 std::string key = iter->first;
1812 p_gc_node = arv_device_get_feature(
p_device_, key.c_str());
1813 if (p_gc_node && arv_gc_feature_node_is_implemented(ARV_GC_FEATURE_NODE(p_gc_node), &error))
1819 switch (iter->second.getType())
1823 int value = (bool)iter->second;
1824 arv_device_set_integer_feature_value(
p_device_, key.c_str(), value);
1825 ROS_INFO(
"Read parameter (bool) %s: %d", key.c_str(), value);
1831 int value = (int)iter->second;
1832 arv_device_set_integer_feature_value(
p_device_, key.c_str(), value);
1833 ROS_INFO(
"Read parameter (int) %s: %d", key.c_str(), value);
1839 double value = (double)iter->second;
1840 arv_device_set_float_feature_value(
p_device_, key.c_str(), value);
1841 ROS_INFO(
"Read parameter (float) %s: %f", key.c_str(), value);
1847 std::string value = (std::string)iter->second;
1848 arv_device_set_string_feature_value(
p_device_, key.c_str(), value.c_str());
1849 ROS_INFO(
"Read parameter (string) %s: %s", key.c_str(), value.c_str());
1859 ROS_WARN(
"Unhandled rosparam type in writeCameraFeaturesFromRosparam()");
1868 nodelet::NodeletUnload unload_service;
1869 unload_service.request.name = this->
getName();