49 #include <rc_genicam_api/config.h>
59 #include <rc_common_msgs/ReturnCodeConstants.h>
60 #include <rc_common_msgs/CameraParam.h>
101 if (ns.size() > 0 && ns[0] ==
'/')
120 std::string
id =
"*";
121 std::string access =
"control";
123 pnh.
param(
"device",
id,
id);
124 pnh.
param(
"gev_access", access, access);
127 if (access ==
"exclusive")
131 else if (access ==
"control")
160 rc_common_msgs::Trigger::Response& res)
162 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
166 if (
config.depth_acquisition_mode !=
"Continuous")
174 res.return_code.value = rc_common_msgs::ReturnCodeConstants::SUCCESS;
175 res.return_code.message =
"Stereo matching was triggered.";
177 catch (
const std::exception& ex)
179 res.return_code.value = rc_common_msgs::ReturnCodeConstants::INTERNAL_ERROR;
180 res.return_code.message = ex.what();
186 res.return_code.value = rc_common_msgs::ReturnCodeConstants::NOT_APPLICABLE;
187 res.return_code.message =
"Triggering stereo matching is only possible if acquisition_mode is set to SingleFrame "
188 "or SingleFrameOut1!";
194 res.return_code.value = rc_common_msgs::ReturnCodeConstants::NOT_APPLICABLE;
195 res.return_code.message =
"Not connected";
203 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
215 config.camera_exp_control =
"Manual";
216 config.camera_exp_auto_mode =
"Normal";
220 config.camera_exp_control =
"HDR";
221 config.camera_exp_auto_mode =
"Normal";
225 config.camera_exp_control =
"Auto";
227 if (v ==
"Continuous")
229 config.camera_exp_auto_mode =
"Normal";
233 config.camera_exp_auto_mode = v;
244 catch (
const std::exception&)
246 config.camera_exp_auto_average_max = 0.75f;
247 config.camera_exp_auto_average_min = 0.25f;
260 if (
config.camera_gamma == 0)
269 config.camera_wb_auto = (v !=
"Off");
275 catch (
const std::exception&)
277 config.camera_wb_auto =
true;
278 config.camera_wb_ratio_red = 1.2;
279 config.camera_wb_ratio_blue = 2.4;
308 catch (
const std::exception&)
310 NODELET_WARN(
"rc_visard_driver: rc_visard has an older firmware, depth_exposure_adapt_timeout is not available.");
317 pnh.
param(
"camera_exp_control",
config.camera_exp_control,
config.camera_exp_control);
318 pnh.
param(
"camera_exp_auto_mode",
config.camera_exp_auto_mode,
config.camera_exp_auto_mode);
320 pnh.
param(
"camera_exp_auto_average_max",
config.camera_exp_auto_average_max,
config.camera_exp_auto_average_max);
321 pnh.
param(
"camera_exp_auto_average_min",
config.camera_exp_auto_average_min,
config.camera_exp_auto_average_min);
323 pnh.
param(
"camera_gain_value",
config.camera_gain_value,
config.camera_gain_value);
325 pnh.
param(
"camera_exp_offset_x",
config.camera_exp_offset_x,
config.camera_exp_offset_x);
326 pnh.
param(
"camera_exp_offset_y",
config.camera_exp_offset_y,
config.camera_exp_offset_y);
328 pnh.
param(
"camera_exp_height",
config.camera_exp_height,
config.camera_exp_height);
330 pnh.
param(
"camera_wb_ratio_red",
config.camera_wb_ratio_red,
config.camera_wb_ratio_red);
331 pnh.
param(
"camera_wb_ratio_blue",
config.camera_wb_ratio_blue,
config.camera_wb_ratio_blue);
332 pnh.
param(
"depth_acquisition_mode",
config.depth_acquisition_mode,
config.depth_acquisition_mode);
334 pnh.
param(
"depth_static_scene",
config.depth_static_scene,
config.depth_static_scene);
335 pnh.
param(
"depth_double_shot",
config.depth_double_shot,
config.depth_double_shot);
342 pnh.
param(
"depth_maxdeptherr",
config.depth_maxdeptherr,
config.depth_maxdeptherr);
343 pnh.
param(
"depth_exposure_adapt_timeout",
config.depth_exposure_adapt_timeout,
config.depth_exposure_adapt_timeout);
352 pnh.
setParam(
"camera_exp_auto_mode",
config.camera_exp_auto_mode);
354 pnh.
setParam(
"camera_exp_auto_average_max",
config.camera_exp_auto_average_max);
355 pnh.
setParam(
"camera_exp_auto_average_min",
config.camera_exp_auto_average_min);
365 pnh.
setParam(
"camera_wb_ratio_blue",
config.camera_wb_ratio_blue);
366 pnh.
setParam(
"depth_acquisition_mode",
config.depth_acquisition_mode);
377 pnh.
setParam(
"depth_exposure_adapt_timeout",
config.depth_exposure_adapt_timeout);
390 inline double setFloatLimited(
const std::shared_ptr<GenApi::CNodeMapRef> &nodemap,
391 const char *name,
double value)
398 value = std::max(vmin, std::min(value, vmax));
405 inline int setIntegerLimited(
const std::shared_ptr<GenApi::CNodeMapRef> &nodemap,
406 const char *name,
int value)
413 value =
static_cast<int>(std::max(vmin, std::min(
static_cast<int64_t>(value), vmax)));
424 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
432 c.camera_fps = setFloatLimited(
nodemap,
"AcquisitionFrameRate", c.camera_fps);
437 if (c.camera_exp_control ==
"HDR")
442 c.camera_exp_control =
"Auto";
446 if (c.camera_exp_control ==
"Auto")
450 if (c.camera_exp_auto_mode ==
"Off" || c.camera_exp_auto_mode ==
"Normal")
452 c.camera_exp_auto_mode =
"Continuous";
457 std::vector<std::string> list;
460 std::string mode =
"Continuous";
461 for (
size_t i = 0; i < list.size(); i++)
463 if (c.camera_exp_auto_mode == list[i])
475 if (mode ==
"Continuous")
480 c.camera_exp_auto_mode = mode;
482 else if (c.camera_exp_control !=
"HDR")
484 c.camera_exp_control =
"Manual";
497 c.camera_exp_max = setFloatLimited(
nodemap,
"ExposureTimeAutoMax",
498 1000000 * c.camera_exp_max)/1000000;
505 NODELET_WARN(
"rc_visard does not support parameter 'exp_auto_average_max'");
506 c.camera_exp_auto_average_max = 0.75f;
514 NODELET_WARN(
"rc_visard does not support parameter 'exp_auto_average_min'");
515 c.camera_exp_auto_average_min = 0.25f;
521 c.camera_exp_value = setFloatLimited(
nodemap,
"ExposureTime",
522 1000000 * c.camera_exp_value)/1000000;
525 c.camera_gain_value =
round(c.camera_gain_value / 6) * 6;
529 c.camera_gain_value = setFloatLimited(
nodemap,
"Gain", c.camera_gain_value);
532 if (level & 536870912)
536 if (c.camera_gamma != 1.0)
539 c.camera_gamma = 1.0;
546 c.camera_exp_offset_x = setIntegerLimited(
nodemap,
"ExposureRegionOffsetX",
547 c.camera_exp_offset_x);
552 c.camera_exp_offset_y = setIntegerLimited(
nodemap,
"ExposureRegionOffsetY",
553 c.camera_exp_offset_y);
558 c.camera_exp_width = setIntegerLimited(
nodemap,
"ExposureRegionWidth",
564 c.camera_exp_height = setIntegerLimited(
nodemap,
"ExposureRegionHeight",
565 c.camera_exp_height);
568 bool color_ok =
true;
572 if (c.camera_wb_auto)
604 c.camera_wb_auto =
true;
605 c.camera_wb_ratio_red = 1.2;
606 c.camera_wb_ratio_blue = 2.4;
613 if (c.depth_acquisition_mode ==
"S" || c.depth_acquisition_mode ==
"SingleFrame")
615 c.depth_acquisition_mode =
"SingleFrame";
617 else if (c.depth_acquisition_mode ==
"O" || c.depth_acquisition_mode ==
"SingleFrameOut1")
619 c.depth_acquisition_mode =
"SingleFrameOut1";
621 else if (c.depth_acquisition_mode ==
"C" || c.depth_acquisition_mode ==
"Continuous")
623 c.depth_acquisition_mode =
"Continuous";
627 c.depth_acquisition_mode =
"Continuous";
635 if (c.depth_quality ==
"Full" || c.depth_quality ==
"F")
637 c.depth_quality =
"Full";
639 else if (c.depth_quality ==
"High" || c.depth_quality ==
"H")
641 c.depth_quality =
"High";
643 else if (c.depth_quality ==
"Medium" || c.depth_quality ==
"M")
645 c.depth_quality =
"Medium";
647 else if (c.depth_quality ==
"Low" || c.depth_quality ==
"L")
649 c.depth_quality =
"Low";
653 c.depth_quality =
"High";
660 catch (
const std::exception&)
662 c.depth_quality =
"High";
665 NODELET_ERROR(
"Cannot set full quality. Sensor may have no 'stereo_plus' license!");
680 catch (
const std::exception&)
682 c.depth_double_shot =
false;
683 NODELET_ERROR(
"Cannot set double shot mode. Please update the sensor to version >= 20.11.0!");
692 if (level & 524288 && c.depth_smooth !=
config.depth_smooth)
698 catch (
const std::exception&)
700 c.depth_smooth =
false;
703 NODELET_ERROR(
"Cannot switch on smoothing. Sensor may have no 'stereo_plus' license!");
727 if (level & 16777216)
732 if ((level & 33554432) && c.ptp_enabled !=
config.ptp_enabled)
737 c.ptp_enabled =
false;
741 if ((level & 67108864) && c.out1_mode !=
config.out1_mode)
743 if (c.out1_mode !=
"Low" && c.out1_mode !=
"High" && c.out1_mode !=
"ExposureActive" &&
744 c.out1_mode !=
"ExposureAlternateActive")
754 NODELET_ERROR(
"Cannot change out1 mode. Sensor may have no 'iocontrol' license!");
758 if (level & 134217728 && c.out2_mode !=
config.out2_mode)
760 if (c.out2_mode !=
"Low" && c.out2_mode !=
"High" && c.out2_mode !=
"ExposureActive" &&
761 c.out2_mode !=
"ExposureAlternateActive")
771 NODELET_ERROR(
"Cannot change out2 mode. Sensor may have no 'iocontrol' license!");
775 if (level & 268435456)
777 if (!
rcg::setFloat(
nodemap,
"DepthExposureAdaptTimeout", c.depth_exposure_adapt_timeout,
false))
779 c.depth_exposure_adapt_timeout = 0.0;
780 NODELET_ERROR(
"Cannot set depth_exposure_adapt_timeout. Please update the sensor to version >= 21.10!");
785 catch (
const std::exception& ex)
795 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
804 p->requiresComponents(rcomponents, rcolor);
827 for (
size_t i = 0; comp[i].name != 0; i++)
829 if (((rcomponents ^
scomponents) & comp[i].flag) || force)
834 const char* status =
"disabled";
835 if (rcomponents & comp[i].flag)
847 if (rcolor !=
scolor || force)
849 std::string format =
"Mono8";
884 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Disconnected");
899 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Streaming");
904 stat.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"No data");
910 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Idle");
918 stat.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Unknown");
922 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Info");
933 std::vector<std::shared_ptr<rcg::Device> > getSupportedDevices(
const std::string& devid,
934 const std::vector<std::string>& iname)
937 std::vector<std::shared_ptr<rcg::Device> > ret;
939 for (
size_t i = 0; i < system.size(); i++)
943 std::vector<std::shared_ptr<rcg::Interface> > interf = system[i]->getInterfaces();
945 for (
size_t k = 0; k < interf.size(); k++)
947 if (interf[k]->getTLType() ==
"GEV" &&
948 (iname.size() == 0 || std::find(iname.begin(), iname.end(), interf[k]->getID()) != iname.end()))
952 std::vector<std::shared_ptr<rcg::Device> > device = interf[k]->getDevices();
954 for (
size_t j = 0; j < device.size(); j++)
956 if ((device[j]->getVendor() ==
"Roboception GmbH" ||
957 device[j]->getModel().substr(0, 9) ==
"rc_visard" || device[j]->getModel().substr(0, 7) ==
"rc_cube") &&
958 (devid ==
"*" || device[j]->getID() == devid || device[j]->getSerialNumber() == devid ||
959 device[j]->getDisplayName() == devid))
961 ret.push_back(device[j]);
975 class NoDeviceException :
public std::invalid_argument
978 NoDeviceException(
const char* msg) :
std::invalid_argument(
msg)
983 void split(std::vector<std::string>& list,
const std::string& s,
char delim,
bool skip_empty =
true)
985 std::stringstream in(s);
988 while (
getline(in, elem, delim))
990 if (!skip_empty || elem.size() > 0)
992 list.push_back(elem);
1025 std::shared_ptr<GenApi::CChunkAdapter> chunkadapter;
1028 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
1032 std::vector<std::string> iname;
1033 std::string dname = id;
1036 size_t i = dname.find(
':');
1037 if (i != std::string::npos)
1041 iname.push_back(
id.substr(0, i));
1044 dname = dname.substr(i + 1);
1048 std::vector<std::shared_ptr<rcg::Device> > devices = getSupportedDevices(dname, iname);
1050 if (devices.size() == 0)
1052 throw NoDeviceException((
"Cannot find device '" +
id +
"'").c_str());
1055 if (devices.size() > 1)
1057 throw std::invalid_argument(
"Too many devices, please specify unique ID");
1068 std::vector<std::string> list;
1071 if (list.size() < 3 || std::stoi(list[0]) < 20 || (std::stoi(list[0]) == 20 && std::stoi(list[1]) < 4))
1074 throw std::invalid_argument(
"Device version must be 20.04 or higher: " +
device_version);
1081 throw std::invalid_argument(
"Device is not yet ready");
1094 <<
dev->getDisplayName());
1107 reconfig =
new dynamic_reconfigure::Server<rc_genicam_driver::rc_genicam_driverConfig>(
1125 dynamic_reconfigure::Server<rc_genicam_driver::rc_genicam_driverConfig>::CallbackType cb;
1145 std::vector<std::string> formats;
1148 for (
auto&& format : formats)
1150 if (format ==
"YCbCr411_8")
1157 if (format ==
"RGB8")
1166 bool iocontrol_avail =
nodemap->_GetNode(
"LineSource")->GetAccessMode() ==
GenApi::RW;
1170 NODELET_INFO(
"Not a color camera. wb_auto, wb_ratio_red and wb_ratio_blue are without "
1176 NODELET_WARN(
"No stereo_plus license on device. quality=full and smoothing is not available.");
1179 if (!iocontrol_avail)
1181 NODELET_WARN(
"No iocontrol license on device. out1_mode and out2_mode are without function.");
1194 pub.push_back(std::make_shared<CameraInfoPublisher>(nh,
frame_id,
true, callback));
1195 pub.push_back(std::make_shared<CameraInfoPublisher>(nh,
frame_id,
false, callback));
1196 pub.push_back(std::make_shared<CameraParamPublisher>(nh,
frame_id,
true, callback));
1197 pub.push_back(std::make_shared<CameraParamPublisher>(nh,
frame_id,
false, callback));
1199 pub.push_back(std::make_shared<ImagePublisher>(it,
frame_id,
true,
false, iocontrol_avail, callback));
1200 pub.push_back(std::make_shared<ImagePublisher>(it,
frame_id,
false,
false, iocontrol_avail, callback));
1204 pub.push_back(std::make_shared<ImagePublisher>(it,
frame_id,
true,
true, iocontrol_avail, callback));
1205 pub.push_back(std::make_shared<ImagePublisher>(it,
frame_id,
false,
true, iocontrol_avail, callback));
1208 pub.push_back(std::make_shared<DisparityPublisher>(nh,
frame_id, callback));
1209 pub.push_back(std::make_shared<DisparityColorPublisher>(it,
frame_id, callback));
1210 pub.push_back(std::make_shared<DepthPublisher>(nh,
frame_id, callback));
1212 pub.push_back(std::make_shared<ConfidencePublisher>(nh,
frame_id, callback));
1213 pub.push_back(std::make_shared<ErrorDisparityPublisher>(nh,
frame_id, callback));
1214 pub.push_back(std::make_shared<ErrorDepthPublisher>(nh,
frame_id, callback));
1216 pub.push_back(std::make_shared<Points2Publisher>(nh,
frame_id, callback));
1220 for (
auto&& p :
pub)
1231 std::vector<std::shared_ptr<rcg::Stream> > stream =
dev->getStreams();
1233 if (stream.size() == 0)
1235 throw std::invalid_argument(
"Device does not offer streams");
1239 stream[0]->startStreaming();
1254 std::string out1_mode_on_sensor;
1265 out1_mode_on_sensor =
"";
1271 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
1280 chunkadapter->AttachBuffer(
reinterpret_cast<std::uint8_t*
>(buffer->
getGlobalBase()),
1291 for (uint32_t part = 0; part < npart; part++)
1296 for (
auto&& p :
pub)
1298 p->publish(buffer, part, pixelformat);
1305 chunkadapter->DetachBuffer();
1316 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
1322 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
1328 if (out1_mode_on_sensor.size() == 0)
1331 out1_mode_on_sensor =
config.out1_mode;
1334 if (out1_mode_on_sensor !=
config.out1_mode)
1336 config.out1_mode = out1_mode_on_sensor;
1348 stream[0]->stopStreaming();
1365 catch (
const NoDeviceException& ex)
1379 catch (
const std::exception& ex)
1411 std::lock_guard<std::recursive_mutex> lock(
device_mtx);
1421 catch (
const std::exception& ex)