SickSafetyscannersRos.cpp
Go to the documentation of this file.
1 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2 
3 // -- BEGIN LICENSE BLOCK ----------------------------------------------
4 
24 // -- END LICENSE BLOCK ------------------------------------------------
25 
26 //----------------------------------------------------------------------
33 //----------------------------------------------------------------------
34 
35 
37 
38 
39 namespace sick {
40 
42  : m_private_nh("~")
43  , m_initialised(false)
44  , m_time_offset(0.0)
45  , m_range_min(0.0)
46  , m_range_max(0.0)
47  , m_angle_offset(-90.0)
48  , m_use_pers_conf(false)
49 {
50  dynamic_reconfigure::Server<
51  sick_safetyscanners::SickSafetyscannersConfigurationConfig>::CallbackType reconf_callback =
52  boost::bind(&SickSafetyscannersRos::reconfigureCallback, this, _1, _2);
53  m_dynamic_reconfiguration_server.setCallback(reconf_callback);
54  if (!readParameters())
55  {
56  ROS_ERROR("Could not read parameters.");
58  }
59  // tcp port can not be changed in the sensor configuration, therefore it is hardcoded
61  m_laser_scan_publisher = m_nh.advertise<sensor_msgs::LaserScan>("scan", 100);
63  m_nh.advertise<sick_safetyscanners::ExtendedLaserScanMsg>("extended_laser_scan", 100);
64  m_raw_data_publisher = m_nh.advertise<sick_safetyscanners::RawMicroScanDataMsg>("raw_data", 100);
66  m_nh.advertise<sick_safetyscanners::OutputPathsMsg>("output_paths", 100);
69 
70  // Diagnostics for frequency
72 
78  m_laser_scan_publisher, m_diagnostic_updater, frequency_param, timestamp_param));
80 
81  m_device = std::make_shared<sick::SickSafetyscanners>(
82  boost::bind(&SickSafetyscannersRos::receivedUDPPacket, this, _1),
85  m_device->run();
87 
88  if (m_use_pers_conf)
89  {
91  }
92 
93  m_device->changeSensorSettings(m_communication_settings);
94  m_initialised = true;
95  ROS_INFO("Successfully launched node.");
96 }
97 
99 {
100  ROS_INFO("Reading Type code settings");
102  m_device->requestTypeCode(m_communication_settings, type_code);
104  m_range_min = 0.1;
105  m_range_max = type_code.getMaxRange();
106 }
107 
109 {
110  ROS_INFO("Reading Persistent Configuration");
112  m_device->requestPersistentConfig(m_communication_settings, config_data);
115 }
116 
118  const sick_safetyscanners::SickSafetyscannersConfigurationConfig& config, const uint32_t& level)
119 {
120  if (isInitialised())
121  {
122  m_communication_settings.setEnabled(config.channel_enabled);
124  if (config.angle_start == config.angle_end)
125  {
128  }
129  else
130  {
133  }
134  m_communication_settings.setFeatures(config.general_system_state,
135  config.derived_settings,
136  config.measurement_data,
137  config.intrusion_data,
138  config.application_io_data);
139  m_device->changeSensorSettings(m_communication_settings);
140 
141  m_frame_id = config.frame_id;
142 
143  m_time_offset = config.time_offset;
144 
145  m_expected_frequency = config.expected_frequency;
146 
147  m_min_intensities = config.min_intensities;
148  }
149 }
150 
152 {
153  return m_initialised;
154 }
155 
156 
158 
160 {
161  std::string sensor_ip_adress = "192.168.1.10";
162  if (!m_private_nh.getParam("sensor_ip", sensor_ip_adress))
163  {
164  // sensor_ip_adress = sick_safetyscanners::SickSafetyscannersConfiguration_sensor_ip;
165  ROS_WARN("Using default sensor IP: %s", sensor_ip_adress.c_str());
166  }
167  m_communication_settings.setSensorIp(sensor_ip_adress);
168 
169 
170  std::string host_ip_adress = "192.168.1.9";
171  if (!m_private_nh.getParam("host_ip", host_ip_adress))
172  {
173  ROS_WARN("Using default host IP: %s", host_ip_adress.c_str());
174  }
175  m_communication_settings.setHostIp(host_ip_adress);
176 
177  std::string interface_ip_adress = "0.0.0.0";
178  if (!m_private_nh.getParam("interface_ip", interface_ip_adress))
179  {
180  ROS_WARN("Using default interface IP: %s", interface_ip_adress.c_str());
181  }
182  m_interface_ip = boost::asio::ip::address_v4::from_string(interface_ip_adress);
183 
184  int host_udp_port = 0;
185  if (!m_private_nh.getParam("host_udp_port", host_udp_port))
186  {
187  ROS_WARN("Using default host UDP Port: %i", host_udp_port);
188  }
190 
191  ROS_WARN("If not further specified the default values for the dynamic reconfigurable parameters "
192  "will be loaded.");
193 
194 
195  int channel = 0;
196  m_private_nh.getParam("channel", channel);
198 
199  bool enabled;
200  m_private_nh.getParam("channel_enabled", enabled);
202 
203  int skip;
204  m_private_nh.getParam("skip", skip);
206 
207  float angle_start;
208  m_private_nh.getParam("angle_start", angle_start);
209 
210  float angle_end;
211  m_private_nh.getParam("angle_end", angle_end);
212 
213  m_private_nh.getParam("frequency_tolerance", m_frequency_tolerance);
214  m_private_nh.getParam("expected_frequency", m_expected_frequency);
215  m_private_nh.getParam("timestamp_min_acceptable", m_timestamp_min_acceptable);
216  m_private_nh.getParam("timestamp_max_acceptable", m_timestamp_max_acceptable);
217 
218  // Included check before calculations to prevent rounding errors while calculating
219  if (angle_start == angle_end)
220  {
223  }
224  else
225  {
228  }
229 
230  m_private_nh.getParam("time_offset", m_time_offset);
231 
232  bool general_system_state;
233  m_private_nh.getParam("general_system_state", general_system_state);
234 
235  bool derived_settings;
236  m_private_nh.getParam("derived_settings", derived_settings);
237 
238  bool measurement_data;
239  m_private_nh.getParam("measurement_data", measurement_data);
240 
241  bool intrusion_data;
242  m_private_nh.getParam("intrusion_data", intrusion_data);
243 
244  bool application_io_data;
245  m_private_nh.getParam("application_io_data", application_io_data);
246 
248  general_system_state, derived_settings, measurement_data, intrusion_data, application_io_data);
249 
250  m_private_nh.getParam("frame_id", m_frame_id);
251 
252  m_private_nh.getParam("use_persistent_config", m_use_pers_conf);
253 
254  m_private_nh.getParam("min_intensities", m_min_intensities);
255 
256  return true;
257 }
258 
260 {
261  if (!data.getMeasurementDataPtr()->isEmpty() && !data.getDerivedValuesPtr()->isEmpty())
262  {
263  sensor_msgs::LaserScan scan = createLaserScanMessage(data);
264 
265  m_diagnosed_laser_scan_publisher->publish(scan);
266  }
267 
268 
269  if (!data.getMeasurementDataPtr()->isEmpty() && !data.getDerivedValuesPtr()->isEmpty())
270  {
271  sick_safetyscanners::ExtendedLaserScanMsg extended_scan = createExtendedLaserScanMessage(data);
272 
274 
275  sick_safetyscanners::OutputPathsMsg output_paths = createOutputPathsMessage(data);
276  m_output_path_publisher.publish(output_paths);
277  }
278 
281 
283 }
284 
285 std::string boolToString(bool b)
286 {
287  return b ? "true" : "false";
288 }
289 
292 {
293  const sick_safetyscanners::DataHeaderMsg& header = m_last_raw_data.header;
294  if (header.timestamp_time == 0 && header.timestamp_date == 0)
295  {
296  diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::STALE,
297  "Could not get sensor state");
298  return;
299  }
300 
301  diagnostic_status.addf("Version version", "%c", header.version_version);
302  diagnostic_status.addf("Version major version", "%u", header.version_major_version);
303  diagnostic_status.addf("Version minor version", "%u", header.version_minor_version);
304  diagnostic_status.addf("Version release", "%u", header.version_release);
305  diagnostic_status.addf("Serial number of device", "%u", header.serial_number_of_device);
306  diagnostic_status.addf(
307  "Serial number of channel plug", "%u", header.serial_number_of_channel_plug);
308  diagnostic_status.addf("Channel number", "%u", header.channel_number);
309  diagnostic_status.addf("Sequence number", "%u", header.sequence_number);
310  diagnostic_status.addf("Scan number", "%u", header.scan_number);
311  diagnostic_status.addf("Timestamp date", "%u", header.timestamp_date);
312  diagnostic_status.addf("Timestamp time", "%u", header.timestamp_time);
313 
314  const sick_safetyscanners::GeneralSystemStateMsg& state = m_last_raw_data.general_system_state;
315  diagnostic_status.add("Run mode active", boolToString(state.run_mode_active));
316  diagnostic_status.add("Standby mode active", boolToString(state.standby_mode_active));
317  diagnostic_status.add("Contamination warning", boolToString(state.contamination_warning));
318  diagnostic_status.add("Contamination error", boolToString(state.contamination_error));
319  diagnostic_status.add("Reference contour status", boolToString(state.reference_contour_status));
320  diagnostic_status.add("Manipulation status", boolToString(state.manipulation_status));
321  diagnostic_status.addf(
322  "Current monitoring case no table 1", "%u", state.current_monitoring_case_no_table_1);
323  diagnostic_status.addf(
324  "Current monitoring case no table 2", "%u", state.current_monitoring_case_no_table_2);
325  diagnostic_status.addf(
326  "Current monitoring case no table 3", "%u", state.current_monitoring_case_no_table_3);
327  diagnostic_status.addf(
328  "Current monitoring case no table 4", "%u", state.current_monitoring_case_no_table_4);
329  diagnostic_status.add("Application error", boolToString(state.application_error));
330  diagnostic_status.add("Device error", boolToString(state.device_error));
331 
332  if (state.device_error)
333  {
334  diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Device error");
335  }
336  else
337  {
338  diagnostic_status.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
339  }
340 }
341 
342 sick_safetyscanners::ExtendedLaserScanMsg
344 {
345  sensor_msgs::LaserScan scan = createLaserScanMessage(data);
346  sick_safetyscanners::ExtendedLaserScanMsg msg;
347  msg.laser_scan = scan;
348 
349  std::vector<sick::datastructure::ScanPoint> scan_points =
350  data.getMeasurementDataPtr()->getScanPointsVector();
351  uint32_t num_scan_points = scan_points.size();
352 
353 
354  msg.reflektor_status.resize(num_scan_points);
355  msg.intrusion.resize(num_scan_points);
356  msg.reflektor_median.resize(num_scan_points);
357  std::vector<bool> medians = getMedianReflectors(scan_points);
358  for (uint32_t i = 0; i < num_scan_points; ++i)
359  {
360  const sick::datastructure::ScanPoint scan_point = scan_points.at(i);
361  msg.reflektor_status[i] = scan_point.getReflectorBit();
362  msg.intrusion[i] = scan_point.getContaminationBit();
363  msg.reflektor_median[i] = medians.at(i);
364  }
365  return msg;
366 }
367 
369  const std::vector<sick::datastructure::ScanPoint> scan_points)
370 {
371  std::vector<bool> res;
372  res.resize(scan_points.size());
373  bool last = false;
374  int start = -1;
375  for (size_t i = 0; i < scan_points.size(); i++)
376  {
377  res.at(i) = false;
378  if (!last && scan_points.at(i).getReflectorBit())
379  {
380  last = true;
381  start = i;
382  }
383  else if (last && (!scan_points.at(i).getReflectorBit() || i == scan_points.size() - 1))
384  {
385  last = false;
386  res.at(start + ((i - start) / 2)) = true;
387  }
388  }
389 
390  return res;
391 }
392 
393 sensor_msgs::LaserScan
395 {
396  sensor_msgs::LaserScan scan;
397  scan.header.frame_id = m_frame_id;
398  scan.header.stamp = ros::Time::now();
399  // Add time offset (to account for network latency etc.)
400  scan.header.stamp += ros::Duration().fromSec(m_time_offset);
401  // TODO check why returned number of beams is misaligned to size of vector
402  std::vector<sick::datastructure::ScanPoint> scan_points =
403  data.getMeasurementDataPtr()->getScanPointsVector();
404  uint32_t num_scan_points = scan_points.size();
405 
406  scan.angle_min = sick::degToRad(data.getDerivedValuesPtr()->getStartAngle() + m_angle_offset);
407  double angle_max =
409  ->getScanPointsVector()
410  .at(data.getMeasurementDataPtr()->getScanPointsVector().size() - 1)
411  .getAngle() +
413  scan.angle_max = angle_max;
414  scan.angle_increment = sick::degToRad(data.getDerivedValuesPtr()->getAngularBeamResolution());
415  boost::posix_time::microseconds time_increment =
416  boost::posix_time::microseconds(data.getDerivedValuesPtr()->getInterbeamPeriod());
417  scan.time_increment = time_increment.total_microseconds() * 1e-6;
418  boost::posix_time::milliseconds scan_time =
419  boost::posix_time::milliseconds(data.getDerivedValuesPtr()->getScanTime());
420  scan.scan_time = scan_time.total_microseconds() * 1e-6;
421  scan.range_min = m_range_min;
422  scan.range_max = m_range_max;
423  scan.ranges.resize(num_scan_points);
424  scan.intensities.resize(num_scan_points);
425 
426 
427  for (uint32_t i = 0; i < num_scan_points; ++i)
428  {
429  const sick::datastructure::ScanPoint scan_point = scan_points.at(i);
430  // Filter for intensities
431  if (m_min_intensities < static_cast<double>(scan_point.getReflectivity()))
432  {
433  scan.ranges[i] = static_cast<float>(scan_point.getDistance()) *
434  data.getDerivedValuesPtr()->getMultiplicationFactor() * 1e-3; // mm -> m
435  // Set values close to/greater than max range to infinity according to REP 117
436  // https://www.ros.org/reps/rep-0117.html
437  if (scan.ranges[i] >= (0.999 * m_range_max))
438  {
439  scan.ranges[i] = std::numeric_limits<double>::infinity();
440  }
441  }
442  else
443  {
444  scan.ranges[i] = std::numeric_limits<double>::infinity();
445  }
446  scan.intensities[i] = static_cast<float>(scan_point.getReflectivity());
447  }
448  return scan;
449 }
450 
451 sick_safetyscanners::OutputPathsMsg
453 {
454  sick_safetyscanners::OutputPathsMsg msg;
455 
456  std::shared_ptr<sick::datastructure::ApplicationData> app_data = data.getApplicationDataPtr();
457  sick::datastructure::ApplicationOutputs outputs = app_data->getOutputs();
458 
459  std::vector<bool> eval_out = outputs.getEvalOutVector();
460  std::vector<bool> eval_out_is_safe = outputs.getEvalOutIsSafeVector();
461  std::vector<bool> eval_out_valid = outputs.getEvalOutIsValidVector();
462 
463  std::vector<uint16_t> monitoring_case_numbers = outputs.getMonitoringCaseVector();
464  std::vector<bool> monitoring_case_number_flags = outputs.getMonitoringCaseFlagsVector();
465 
466  // Fix according to issue #46, however why this appears is not clear
467  if (monitoring_case_number_flags.size() > 0)
468  {
469  msg.active_monitoring_case = monitoring_case_numbers.at(0);
470  }
471  else
472  {
473  msg.active_monitoring_case = 0;
474  }
475 
476  for (size_t i = 0; i < eval_out.size(); i++)
477  {
478  msg.status.push_back(eval_out.at(i));
479  msg.is_safe.push_back(eval_out_is_safe.at(i));
480  msg.is_valid.push_back(eval_out_valid.at(i));
481  }
482  return msg;
483 }
484 
485 sick_safetyscanners::RawMicroScanDataMsg
487 {
488  sick_safetyscanners::RawMicroScanDataMsg msg;
489 
490  msg.header = createDataHeaderMessage(data);
491  msg.derived_values = createDerivedValuesMessage(data);
492  msg.general_system_state = createGeneralSystemStateMessage(data);
493  msg.measurement_data = createMeasurementDataMessage(data);
494  msg.intrusion_data = createIntrusionDataMessage(data);
495  msg.application_data = createApplicationDataMessage(data);
496 
497  return msg;
498 }
499 
500 sick_safetyscanners::DataHeaderMsg
502 {
503  sick_safetyscanners::DataHeaderMsg msg;
504 
505  if (!data.getDataHeaderPtr()->isEmpty())
506  {
507  std::shared_ptr<sick::datastructure::DataHeader> data_header = data.getDataHeaderPtr();
508 
509  msg.version_version = data_header->getVersionIndicator();
510  msg.version_release = data_header->getVersionRelease();
511  msg.version_major_version = data_header->getVersionMajorVersion();
512  msg.version_minor_version = data_header->getVersionMinorVersion();
513 
514  msg.scan_number = data_header->getScanNumber();
515  msg.sequence_number = data_header->getSequenceNumber();
516 
517  msg.serial_number_of_device = data_header->getSerialNumberOfDevice();
518  msg.serial_number_of_channel_plug = data_header->getSerialNumberOfSystemPlug();
519 
520  msg.channel_number = data_header->getChannelNumber();
521 
522  msg.timestamp_date = data_header->getTimestampDate();
523  msg.timestamp_time = data_header->getTimestampTime();
524  }
525  return msg;
526 }
527 
528 sick_safetyscanners::DerivedValuesMsg
530 {
531  sick_safetyscanners::DerivedValuesMsg msg;
532 
533  if (!data.getDerivedValuesPtr()->isEmpty())
534  {
535  std::shared_ptr<sick::datastructure::DerivedValues> derived_values = data.getDerivedValuesPtr();
536 
537  msg.multiplication_factor = derived_values->getMultiplicationFactor();
538  msg.scan_time = derived_values->getScanTime();
539  msg.interbeam_period = derived_values->getInterbeamPeriod();
540  msg.number_of_beams = derived_values->getNumberOfBeams();
541  msg.start_angle = derived_values->getStartAngle() + m_angle_offset;
542  msg.angular_beam_resolution = derived_values->getAngularBeamResolution();
543  }
544  return msg;
545 }
546 
547 sick_safetyscanners::GeneralSystemStateMsg
549 {
550  sick_safetyscanners::GeneralSystemStateMsg msg;
551 
552  if (!data.getGeneralSystemStatePtr()->isEmpty())
553  {
554  std::shared_ptr<sick::datastructure::GeneralSystemState> general_system_state =
556 
557  msg.run_mode_active = general_system_state->getRunModeActive();
558  msg.standby_mode_active = general_system_state->getStandbyModeActive();
559  msg.contamination_warning = general_system_state->getContaminationWarning();
560  msg.contamination_error = general_system_state->getContaminationError();
561  msg.reference_contour_status = general_system_state->getReferenceContourStatus();
562  msg.manipulation_status = general_system_state->getManipulationStatus();
563 
564  std::vector<bool> safe_cut_off_path = general_system_state->getSafeCutOffPathVector();
565  for (size_t i = 0; i < safe_cut_off_path.size(); i++)
566  {
567  msg.safe_cut_off_path.push_back(safe_cut_off_path.at(i));
568  }
569 
570  std::vector<bool> non_safe_cut_off_path = general_system_state->getNonSafeCutOffPathVector();
571  for (size_t i = 0; i < non_safe_cut_off_path.size(); i++)
572  {
573  msg.non_safe_cut_off_path.push_back(non_safe_cut_off_path.at(i));
574  }
575 
576  std::vector<bool> reset_required_cut_off_path =
577  general_system_state->getResetRequiredCutOffPathVector();
578  for (size_t i = 0; i < reset_required_cut_off_path.size(); i++)
579  {
580  msg.reset_required_cut_off_path.push_back(reset_required_cut_off_path.at(i));
581  }
582 
583  msg.current_monitoring_case_no_table_1 =
584  general_system_state->getCurrentMonitoringCaseNoTable1();
585  msg.current_monitoring_case_no_table_2 =
586  general_system_state->getCurrentMonitoringCaseNoTable2();
587  msg.current_monitoring_case_no_table_3 =
588  general_system_state->getCurrentMonitoringCaseNoTable3();
589  msg.current_monitoring_case_no_table_4 =
590  general_system_state->getCurrentMonitoringCaseNoTable4();
591 
592  msg.application_error = general_system_state->getApplicationError();
593  msg.device_error = general_system_state->getDeviceError();
594  }
595  return msg;
596 }
597 
598 sick_safetyscanners::MeasurementDataMsg
600 {
601  sick_safetyscanners::MeasurementDataMsg msg;
602 
603  if (!data.getMeasurementDataPtr()->isEmpty())
604  {
605  msg.number_of_beams = data.getMeasurementDataPtr()->getNumberOfBeams();
606  msg.scan_points = createScanPointMessageVector(data);
607  }
608  return msg;
609 }
610 
611 std::vector<sick_safetyscanners::ScanPointMsg>
613 {
614  std::vector<sick_safetyscanners::ScanPointMsg> msg_vector;
615 
616  std::shared_ptr<sick::datastructure::MeasurementData> measurement_data =
617  data.getMeasurementDataPtr();
618  std::vector<sick::datastructure::ScanPoint> scan_points = measurement_data->getScanPointsVector();
619  // uint32_t num_points = measurement_data->getNumberOfBeams();
620  uint32_t num_points = scan_points.size();
621  for (uint32_t i = 0; i < num_points; i++)
622  {
623  sick::datastructure::ScanPoint scan_point = scan_points.at(i);
624  sick_safetyscanners::ScanPointMsg msg;
625  msg.distance = scan_point.getDistance();
626  msg.reflectivity = scan_point.getReflectivity();
627  msg.angle = scan_point.getAngle() + m_angle_offset;
628  msg.valid = scan_point.getValidBit();
629  msg.infinite = scan_point.getInfiniteBit();
630  msg.glare = scan_point.getGlareBit();
631  msg.reflector = scan_point.getReflectorBit();
632  msg.contamination_warning = scan_point.getContaminationWarningBit();
633  msg.contamination = scan_point.getContaminationBit();
634 
635  msg_vector.push_back(msg);
636  }
637  return msg_vector;
638 }
639 
640 sick_safetyscanners::IntrusionDataMsg
642 {
643  sick_safetyscanners::IntrusionDataMsg msg;
644 
645  if (!data.getIntrusionDataPtr()->isEmpty())
646  {
647  msg.data = createIntrusionDatumMessageVector(data);
648  }
649  return msg;
650 }
651 
652 std::vector<sick_safetyscanners::IntrusionDatumMsg>
654 {
655  std::vector<sick_safetyscanners::IntrusionDatumMsg> msg_vector;
656 
657  std::shared_ptr<sick::datastructure::IntrusionData> intrusion_data = data.getIntrusionDataPtr();
658  std::vector<sick::datastructure::IntrusionDatum> intrusion_datums =
659  intrusion_data->getIntrusionDataVector();
660 
661  for (size_t i = 0; i < intrusion_datums.size(); i++)
662  {
663  sick_safetyscanners::IntrusionDatumMsg msg;
664  sick::datastructure::IntrusionDatum intrusion_datum = intrusion_datums.at(i);
665  msg.size = intrusion_datum.getSize();
666  std::vector<bool> flags = intrusion_datum.getFlagsVector();
667  for (size_t j = 0; j < flags.size(); j++)
668  {
669  msg.flags.push_back(flags.at(j));
670  }
671  msg_vector.push_back(msg);
672  }
673  return msg_vector;
674 }
675 
676 sick_safetyscanners::ApplicationDataMsg
678 {
679  sick_safetyscanners::ApplicationDataMsg msg;
680 
681  if (!data.getApplicationDataPtr()->isEmpty())
682  {
683  msg.inputs = createApplicationInputsMessage(data);
684  msg.outputs = createApplicationOutputsMessage(data);
685  }
686  return msg;
687 }
688 
689 sick_safetyscanners::ApplicationInputsMsg
691 {
692  sick_safetyscanners::ApplicationInputsMsg msg;
693 
694  std::shared_ptr<sick::datastructure::ApplicationData> app_data = data.getApplicationDataPtr();
695  sick::datastructure::ApplicationInputs inputs = app_data->getInputs();
696  std::vector<bool> unsafe_inputs = inputs.getUnsafeInputsInputSourcesVector();
697  std::vector<bool> unsafe_inputs_flags = inputs.getUnsafeInputsFlagsVector();
698  for (size_t i = 0; i < unsafe_inputs.size(); i++)
699  {
700  msg.unsafe_inputs_input_sources.push_back(unsafe_inputs.at(i));
701  msg.unsafe_inputs_flags.push_back(unsafe_inputs_flags.at(i));
702  }
703  std::vector<uint16_t> monitoring_case = inputs.getMonitoringCasevector();
704  std::vector<bool> monitoring_case_flags = inputs.getMonitoringCaseFlagsVector();
705  for (size_t i = 0; i < monitoring_case.size(); i++)
706  {
707  msg.monitoring_case_number_inputs.push_back(monitoring_case.at(i));
708  msg.monitoring_case_number_inputs_flags.push_back(monitoring_case_flags.at(i));
709  }
710  msg.linear_velocity_inputs_velocity_0 = inputs.getVelocity0();
711  msg.linear_velocity_inputs_velocity_0_transmitted_safely = inputs.getVelocity0TransmittedSafely();
712  msg.linear_velocity_inputs_velocity_0_valid = inputs.getVelocity0Valid();
713  msg.linear_velocity_inputs_velocity_1 = inputs.getVelocity1();
714  msg.linear_velocity_inputs_velocity_1_transmitted_safely = inputs.getVelocity1TransmittedSafely();
715  msg.linear_velocity_inputs_velocity_1_valid = inputs.getVelocity1Valid();
716 
717  msg.sleep_mode_input = inputs.getSleepModeInput();
718 
719  return msg;
720 }
721 
722 sick_safetyscanners::ApplicationOutputsMsg
724 {
725  sick_safetyscanners::ApplicationOutputsMsg msg;
726 
727  std::shared_ptr<sick::datastructure::ApplicationData> app_data = data.getApplicationDataPtr();
728  sick::datastructure::ApplicationOutputs outputs = app_data->getOutputs();
729 
730  std::vector<bool> eval_out = outputs.getEvalOutVector();
731  std::vector<bool> eval_out_is_safe = outputs.getEvalOutIsSafeVector();
732  std::vector<bool> eval_out_valid = outputs.getEvalOutIsValidVector();
733  for (size_t i = 0; i < eval_out.size(); i++)
734  {
735  msg.evaluation_path_outputs_eval_out.push_back(eval_out.at(i));
736  msg.evaluation_path_outputs_is_safe.push_back(eval_out_is_safe.at(i));
737  msg.evaluation_path_outputs_is_valid.push_back(eval_out_valid.at(i));
738  }
739 
740  std::vector<uint16_t> monitoring_case = outputs.getMonitoringCaseVector();
741  std::vector<bool> monitoring_case_flags = outputs.getMonitoringCaseFlagsVector();
742  for (size_t i = 0; i < monitoring_case.size(); i++)
743  {
744  msg.monitoring_case_number_outputs.push_back(monitoring_case.at(i));
745  msg.monitoring_case_number_outputs_flags.push_back(monitoring_case_flags.at(i));
746  }
747 
748  msg.sleep_mode_output = outputs.getSleepModeOutput();
749  msg.sleep_mode_output_valid = outputs.getFlagsSleepModeOutputIsValid();
750 
751  msg.error_flag_contamination_warning = outputs.getHostErrorFlagContaminationWarning();
752  msg.error_flag_contamination_error = outputs.getHostErrorFlagContaminationError();
753  msg.error_flag_manipulation_error = outputs.getHostErrorFlagManipulationError();
754  msg.error_flag_glare = outputs.getHostErrorFlagGlare();
755  msg.error_flag_reference_contour_intruded = outputs.getHostErrorFlagReferenceContourIntruded();
756  msg.error_flag_critical_error = outputs.getHostErrorFlagCriticalError();
757  msg.error_flags_are_valid = outputs.getFlagsHostErrorFlagsAreValid();
758 
759  msg.linear_velocity_outputs_velocity_0 = outputs.getVelocity0();
760  msg.linear_velocity_outputs_velocity_0_transmitted_safely =
762  msg.linear_velocity_outputs_velocity_0_valid = outputs.getVelocity0Valid();
763  msg.linear_velocity_outputs_velocity_1 = outputs.getVelocity1();
764  msg.linear_velocity_outputs_velocity_1_transmitted_safely =
766  msg.linear_velocity_outputs_velocity_1_valid = outputs.getVelocity1Valid();
767 
768  std::vector<int16_t> resulting_velocities = outputs.getResultingVelocityVector();
769  std::vector<bool> resulting_velocities_flags = outputs.getResultingVelocityIsValidVector();
770 
771  for (size_t i = 0; i < resulting_velocities.size(); i++)
772  {
773  msg.resulting_velocity.push_back(resulting_velocities.at(i));
774  msg.resulting_velocity_flags.push_back(resulting_velocities_flags.at(i));
775  }
776 
777 
778  return msg;
779 }
780 
781 bool SickSafetyscannersRos::getFieldData(sick_safetyscanners::FieldData::Request& req,
782  sick_safetyscanners::FieldData::Response& res)
783 {
784  std::vector<sick::datastructure::FieldData> fields;
785  m_device->requestFieldData(m_communication_settings, fields);
786 
787  for (size_t i = 0; i < fields.size(); i++)
788  {
789  sick::datastructure::FieldData field = fields.at(i);
790  sick_safetyscanners::FieldMsg field_msg;
791 
792  field_msg.start_angle = degToRad(field.getStartAngle() + m_angle_offset);
793  field_msg.angular_resolution = degToRad(field.getAngularBeamResolution());
794  field_msg.protective_field = field.getIsProtectiveField();
795 
796  std::vector<uint16_t> ranges = field.getBeamDistances();
797  for (size_t j = 0; j < ranges.size(); j++)
798  {
799  field_msg.ranges.push_back(static_cast<float>(ranges.at(j)) * 1e-3);
800  }
801 
802  res.fields.push_back(field_msg);
803  }
804 
805  datastructure::DeviceName device_name;
806  m_device->requestDeviceName(m_communication_settings, device_name);
807  res.device_name = device_name.getDeviceName();
808 
809 
810  std::vector<sick::datastructure::MonitoringCaseData> monitoring_cases;
811  m_device->requestMonitoringCases(m_communication_settings, monitoring_cases);
812 
813  for (size_t i = 0; i < monitoring_cases.size(); i++)
814  {
815  sick::datastructure::MonitoringCaseData monitoring_case_data = monitoring_cases.at(i);
816  sick_safetyscanners::MonitoringCaseMsg monitoring_case_msg;
817 
818  monitoring_case_msg.monitoring_case_number = monitoring_case_data.getMonitoringCaseNumber();
819  std::vector<uint16_t> mon_fields = monitoring_case_data.getFieldIndices();
820  std::vector<bool> mon_fields_valid = monitoring_case_data.getFieldsValid();
821  for (size_t j = 0; j < mon_fields.size(); j++)
822  {
823  monitoring_case_msg.fields.push_back(mon_fields.at(j));
824  monitoring_case_msg.fields_valid.push_back(mon_fields_valid.at(j));
825  }
826  res.monitoring_cases.push_back(monitoring_case_msg);
827  }
828 
829  return true;
830 }
831 
832 
833 } // namespace sick
bool getVelocity1TransmittedSafely() const
Gets if the second linear velocity output is transmitted safely.
msg
int16_t getVelocity1() const
Gets the second linear velocity output.
sick_safetyscanners::ApplicationOutputsMsg createApplicationOutputsMessage(const sick::datastructure::Data &data)
float getStartAngle() const
Gets the start angle of the scan.
Definition: ConfigData.cpp:171
sick_safetyscanners::MeasurementDataMsg createMeasurementDataMessage(const sick::datastructure::Data &data)
std::vector< uint16_t > getMonitoringCasevector() const
Gets the monitoring case numbers.
diagnostic_updater::Updater m_diagnostic_updater
sick_safetyscanners::DataHeaderMsg createDataHeaderMessage(const sick::datastructure::Data &data)
void setEInterfaceType(const uint8_t &e_interface_type)
Sets the eInterface type.
void setFeatures(const uint16_t &features)
Set the enabled features.
bool getHostErrorFlagReferenceContourIntruded() const
Gets if a reference contour is intruded.
float getStartAngle() const
Get the start angle of the scan.
Definition: FieldData.cpp:193
bool getHostErrorFlagContaminationError() const
Gets if a contamination error is present.
bool getFlagsSleepModeOutputIsValid() const
Gets if the sleep mode is valid.
ROSCPP_DECL void start()
void publish(const boost::shared_ptr< M > &message) const
bool getInfiniteBit() const
Returns if the scanpoint is infinite.
Definition: ScanPoint.cpp:83
void setSensorTcpPort(const uint16_t &sensor_tcp_port)
Sets the sensor tcp port.
bool getVelocity0TransmittedSafely() const
Gets if first linear velocity input is transmitted safely.
int32_t getSize() const
Returns size of flag vector.
int16_t getVelocity1() const
Gets the second linear velocity input.
std::vector< bool > getResultingVelocityIsValidVector() const
Gets if the resulting velocities are valid.
void summary(unsigned char lvl, const std::string s)
std::shared_ptr< ApplicationData > getApplicationDataPtr() const
Gets the application data.
Definition: Data.cpp:94
bool getIsProtectiveField() const
Returns if a field is a protective field.
Definition: FieldData.cpp:173
bool getHostErrorFlagGlare() const
Gets if glare is present.
std::vector< bool > getEvalOutIsSafeVector() const
Gets if a cut-off path from the output paths is safe.
ros::NodeHandle m_nh
ROS node handle.
float getMaxRange() const
Gets the max range for the scanner.
Definition: TypeCode.cpp:61
void setHardwareID(const std::string &hwid)
void setEnabled(bool enabled)
Sets if the channel is enabled.
void reconfigureCallback(const sick_safetyscanners::SickSafetyscannersConfigurationConfig &config, const uint32_t &level)
Function which is triggered when a dynamic reconfiguration is performed.
sick_safetyscanners::GeneralSystemStateMsg createGeneralSystemStateMessage(const sick::datastructure::Data &data)
uint16_t skipToPublishFrequency(int skip)
Converts a skip value into a "publish frequency" value.
The applications inputs from a udp data packet.
std::vector< uint16_t > getMonitoringCaseVector() const
Gets the currently active monitoring case numbers.
bool getVelocity1TransmittedSafely() const
Gets if second linear velocity input is transmitted safely.
Class containing the data of a single scan point.
Definition: ScanPoint.h:46
void add(const std::string &name, TaskFunction f)
std::vector< sick_safetyscanners::IntrusionDatumMsg > createIntrusionDatumMessageVector(const sick::datastructure::Data &data)
void setEndAngle(const uint32_t &end_angle)
Sets the end angle of the scan.
std::shared_ptr< DiagnosedLaserScanPublisher > m_diagnosed_laser_scan_publisher
Class containing a single IntrusionDatum.
std::vector< uint16_t > getBeamDistances() const
Returns vector with beam distances.
Definition: FieldData.cpp:183
bool getVelocity1Valid() const
Gets if second linear velocity input is valid.
Field data for warning and protective fields.
Definition: FieldData.h:48
void sensorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
ros::ServiceServer m_field_service_server
sick_safetyscanners::RawMicroScanDataMsg createRawDataMessage(const sick::datastructure::Data &data)
bool getContaminationBit() const
Returns if the scanpoint is contaminated.
Definition: ScanPoint.cpp:98
void setPublishingFrequency(const uint16_t &publishing_frequency)
Sets the publishing frequency.
int16_t getVelocity0() const
Gets the first linear velocity output.
#define ROS_WARN(...)
uint16_t getDistance() const
Getter for the distance of the scanpoint.
Definition: ScanPoint.cpp:68
std::vector< bool > getFlagsVector() const
Getter for the flags vector.
std::vector< bool > getUnsafeInputsFlagsVector() const
Gets the flags for the unsafe input sources.
sick_safetyscanners::ApplicationDataMsg createApplicationDataMessage(const sick::datastructure::Data &data)
void addf(const std::string &key, const char *format,...)
int16_t getVelocity0() const
Gets the first linear velocity input.
std::shared_ptr< IntrusionData > getIntrusionDataPtr() const
Gets the intrusion data.
Definition: Data.cpp:84
The data class containing all data blocks of a measurement.
Definition: Data.h:55
dynamic_reconfigure::Server< sick_safetyscanners::SickSafetyscannersConfigurationConfig > m_dynamic_reconfiguration_server
bool getGlareBit() const
Returns if the scanpoint has glare.
Definition: ScanPoint.cpp:88
bool getValidBit() const
Returns if the scanpoint is valid.
Definition: ScanPoint.cpp:78
void setSensorIp(const boost::asio::ip::address_v4 &sensor_ip)
Sets the sensor IP-address.
std::string boolToString(bool b)
std::vector< bool > getEvalOutVector() const
Gets the state of the non safe cut-off paths.
int8_t getSleepModeInput() const
Gets the state of the sleep mode.
sick_safetyscanners::ExtendedLaserScanMsg createExtendedLaserScanMessage(const sick::datastructure::Data &data)
bool getHostErrorFlagContaminationWarning() const
Gets if a contamination warning is present.
sick_safetyscanners::DerivedValuesMsg createDerivedValuesMessage(const sick::datastructure::Data &data)
bool getReflectorBit() const
Returns if the scanpoint detects a reflector.
Definition: ScanPoint.cpp:93
bool getFieldData(sick_safetyscanners::FieldData::Request &req, sick_safetyscanners::FieldData::Response &res)
bool getFlagsHostErrorFlagsAreValid() const
Gets if the error flags are valid.
bool getVelocity0Valid() const
Gets if first linear velocity input is valid.
Duration & fromSec(double t)
void receivedUDPPacket(const datastructure::Data &data)
Function which is called when a new complete UDP Packet is received.
Class containing the device name of a laser scanner.
Definition: DeviceName.h:47
std::string getDeviceName() const
Gets the device name for the scanner.
Definition: DeviceName.cpp:42
#define ROS_INFO(...)
float degToRad(float deg)
Converts degrees to radians.
std::vector< bool > getFieldsValid() const
Returns if the fields are configured and valid.
ros::Publisher m_laser_scan_publisher
ROS topic publisher.
sick_safetyscanners::IntrusionDataMsg createIntrusionDataMessage(const sick::datastructure::Data &data)
std::vector< bool > getMonitoringCaseFlagsVector() const
Gets if the corresponding monitoring case number is valid.
ros::NodeHandle m_private_nh
ROS private node handle.
std::vector< bool > getMedianReflectors(const std::vector< sick::datastructure::ScanPoint > scan_points)
std::shared_ptr< MeasurementData > getMeasurementDataPtr() const
Gets the measurement data.
Definition: Data.cpp:74
void setStartAngle(const uint32_t &start_angle)
Sets the start angle of the scan.
sick_safetyscanners::ApplicationInputsMsg createApplicationInputsMessage(const sick::datastructure::Data &data)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
sick_safetyscanners::OutputPathsMsg createOutputPathsMessage(const sick::datastructure::Data &data)
std::shared_ptr< sick::SickSafetyscanners > m_device
boost::asio::ip::address_v4 getSensorIp() const
Gets the sensor IP-address.
uint8_t getInterfaceType() const
Gets the interface type for the scanner.
Definition: TypeCode.cpp:51
sick::datastructure::CommSettings m_communication_settings
The application outputs from a udp data packet.
Config data for current and persistent sensor config.
Definition: ConfigData.h:50
void setHostUdpPort(const uint16_t &host_udp_port)
Sets the host udp port.
sensor_msgs::LaserScan createLaserScanMessage(const sick::datastructure::Data &data)
Stores the data for the different monitoring cases.
void setChannel(const uint8_t &channel)
Sets the channel of the data.
float radToDeg(float rad)
Converts radians to degrees.
sick_safetyscanners::RawMicroScanDataMsg m_last_raw_data
boost::asio::ip::address_v4 m_interface_ip
diagnostic_updater::DiagnosedPublisher< sensor_msgs::LaserScan > DiagnosedLaserScanPublisher
bool getHostErrorFlagManipulationError() const
Gets if a manipulation error is present.
ROSCPP_DECL void requestShutdown()
virtual ~SickSafetyscannersRos()
~SickSafetyscannersRos Destructor if the SickSafetyscanners ROS
bool getVelocity0TransmittedSafely() const
Gets if the first linear velocity output is transmitted safely.
bool getParam(const std::string &key, std::string &s) const
bool getContaminationWarningBit() const
Returns if there is a contamination warning.
Definition: ScanPoint.cpp:103
static Time now()
Class containing the type code of a laser scanner.
Definition: TypeCode.h:62
std::vector< bool > getMonitoringCaseFlagsVector() const
Gets the monitoring case flags.
std::shared_ptr< GeneralSystemState > getGeneralSystemStatePtr() const
Gets the general system state.
Definition: Data.cpp:53
std::shared_ptr< DataHeader > getDataHeaderPtr() const
Gets the data header.
Definition: Data.cpp:42
bool readParameters()
Reads and verifies the ROS parameters.
void add(const std::string &key, const T &val)
bool getHostErrorFlagCriticalError() const
Gets if a critical error is present.
std::vector< bool > getUnsafeInputsInputSourcesVector() const
Gets the unsafe input sources.
float getEndAngle() const
Gets the end angle of the scan.
Definition: ConfigData.cpp:186
std::vector< bool > getEvalOutIsValidVector() const
If the output path is valid.
ros::Publisher m_extended_laser_scan_publisher
std::vector< uint16_t > getFieldIndices() const
Returns the field indices.
int8_t getSleepModeOutput() const
Gets the state of the sleep mode.
float getAngle() const
Getter for the angle in sensor coordinates.
Definition: ScanPoint.cpp:63
#define ROS_ERROR(...)
bool getVelocity0Valid() const
Gets if the first linear velocity output is valid.
std::vector< sick_safetyscanners::ScanPointMsg > createScanPointMessageVector(const sick::datastructure::Data &data)
void setHostIp(const boost::asio::ip::address_v4 &host_ip)
Sets the IP-address of the host from an IP-address.
uint8_t getReflectivity() const
Getter for the reflectivity value.
Definition: ScanPoint.cpp:73
uint16_t getMonitoringCaseNumber() const
Returns the number of the monitoring case.
std::shared_ptr< DerivedValues > getDerivedValuesPtr() const
Gets the derived values.
Definition: Data.cpp:64
float getAngularBeamResolution() const
Returns the angular resolution between the beams.
Definition: FieldData.cpp:223
bool getVelocity1Valid() const
Gets if the second linear velocity output is valid.
std::vector< int16_t > getResultingVelocityVector() const
Gets the resulting velocity for each monitoring case table.
SickSafetyscannersRos()
Constructor of the SickSafetyscannersRos.


sick_safetyscanners
Author(s): Lennart Puck
autogenerated on Fri Apr 2 2021 02:45:41