43 , m_initialised(false)
47 , m_angle_offset(-90.0)
48 , m_use_pers_conf(false)
50 dynamic_reconfigure::Server<
51 sick_safetyscanners::SickSafetyscannersConfigurationConfig>::CallbackType reconf_callback =
63 m_nh.
advertise<sick_safetyscanners::ExtendedLaserScanMsg>(
"extended_laser_scan", 100);
66 m_nh.
advertise<sick_safetyscanners::OutputPathsMsg>(
"output_paths", 100);
86 m_device = std::make_shared<sick::SickSafetyscanners>(
103 ROS_INFO(
"Successfully launched node.");
108 ROS_INFO(
"Reading Type code settings");
118 ROS_INFO(
"Reading Persistent Configuration");
126 const sick_safetyscanners::SickSafetyscannersConfigurationConfig& config,
const uint32_t& level)
132 if (config.angle_start == config.angle_end)
143 config.derived_settings,
144 config.measurement_data,
145 config.intrusion_data,
146 config.application_io_data);
169 std::string sensor_ip_adress =
"192.168.1.10";
173 ROS_WARN(
"Using default sensor IP: %s", sensor_ip_adress.c_str());
178 std::string host_ip_adress =
"192.168.1.9";
181 ROS_WARN(
"Using default host IP: %s", host_ip_adress.c_str());
185 std::string interface_ip_adress =
"0.0.0.0";
188 ROS_WARN(
"Using default interface IP: %s", interface_ip_adress.c_str());
190 m_interface_ip = boost::asio::ip::address_v4::from_string(interface_ip_adress);
192 int host_udp_port = 0;
195 ROS_WARN(
"Using default host UDP Port: %i", host_udp_port);
199 ROS_WARN(
"If not further specified the default values for the dynamic reconfigurable parameters "
227 if (angle_start == angle_end)
240 bool general_system_state;
243 bool derived_settings;
246 bool measurement_data;
252 bool application_io_data;
256 general_system_state, derived_settings, measurement_data, intrusion_data, application_io_data);
269 if (!
data.getMeasurementDataPtr()->isEmpty() && !
data.getDerivedValuesPtr()->isEmpty())
277 if (!
data.getMeasurementDataPtr()->isEmpty() && !
data.getDerivedValuesPtr()->isEmpty())
295 return b ?
"true" :
"false";
302 if (
header.timestamp_time == 0 &&
header.timestamp_date == 0)
304 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::STALE,
305 "Could not get sensor state");
309 diagnostic_status.
addf(
"Version version",
"%c",
header.version_version);
310 diagnostic_status.
addf(
"Version major version",
"%u",
header.version_major_version);
311 diagnostic_status.
addf(
"Version minor version",
"%u",
header.version_minor_version);
312 diagnostic_status.
addf(
"Version release",
"%u",
header.version_release);
314 diagnostic_status.
addf(
"Serial number of device",
"%u",
header.serial_number_of_device);
315 diagnostic_status.
addf(
316 "Serial number of channel plug",
"%u",
header.serial_number_of_channel_plug);
320 diagnostic_status.
addf(
"Channel number",
"%u",
header.channel_number);
321 diagnostic_status.
addf(
"Sequence number",
"%u",
header.sequence_number);
322 diagnostic_status.
addf(
"Scan number",
"%u",
header.scan_number);
323 diagnostic_status.
addf(
"Timestamp date",
"%u",
header.timestamp_date);
324 diagnostic_status.
addf(
"Timestamp time",
"%u",
header.timestamp_time);
326 const sick_safetyscanners::GeneralSystemStateMsg& state =
m_last_raw_data.general_system_state;
327 diagnostic_status.
add(
"Run mode active",
boolToString(state.run_mode_active));
328 diagnostic_status.
add(
"Standby mode active",
boolToString(state.standby_mode_active));
329 diagnostic_status.
add(
"Contamination warning",
boolToString(state.contamination_warning));
330 diagnostic_status.
add(
"Contamination error",
boolToString(state.contamination_error));
331 diagnostic_status.
add(
"Reference contour status",
boolToString(state.reference_contour_status));
332 diagnostic_status.
add(
"Manipulation status",
boolToString(state.manipulation_status));
333 diagnostic_status.
addf(
334 "Current monitoring case no table 1",
"%u", state.current_monitoring_case_no_table_1);
335 diagnostic_status.
addf(
336 "Current monitoring case no table 2",
"%u", state.current_monitoring_case_no_table_2);
337 diagnostic_status.
addf(
338 "Current monitoring case no table 3",
"%u", state.current_monitoring_case_no_table_3);
339 diagnostic_status.
addf(
340 "Current monitoring case no table 4",
"%u", state.current_monitoring_case_no_table_4);
341 diagnostic_status.
add(
"Application error",
boolToString(state.application_error));
342 diagnostic_status.
add(
"Device error",
boolToString(state.device_error));
344 if (state.device_error)
346 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Device error");
348 else if (state.application_error)
350 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Application error");
352 else if (state.contamination_error)
354 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Contamination error");
356 else if (state.contamination_warning)
358 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::WARN,
"Contamination warning");
362 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"OK");
366 sick_safetyscanners::ExtendedLaserScanMsg
370 sick_safetyscanners::ExtendedLaserScanMsg
msg;
371 msg.laser_scan = scan;
373 std::vector<sick::datastructure::ScanPoint> scan_points =
374 data.getMeasurementDataPtr()->getScanPointsVector();
375 uint32_t num_scan_points = scan_points.size();
378 msg.reflektor_status.resize(num_scan_points);
379 msg.intrusion.resize(num_scan_points);
380 msg.reflektor_median.resize(num_scan_points);
382 for (uint32_t i = 0; i < num_scan_points; ++i)
387 msg.reflektor_median[i] = medians.at(i);
393 const std::vector<sick::datastructure::ScanPoint> scan_points)
395 std::vector<bool> res;
396 res.resize(scan_points.size());
399 for (
size_t i = 0; i < scan_points.size(); i++)
402 if (!last && scan_points.at(i).getReflectorBit())
407 else if (last && (!scan_points.at(i).getReflectorBit() || i == scan_points.size() - 1))
417 sensor_msgs::LaserScan
420 sensor_msgs::LaserScan scan;
426 std::vector<sick::datastructure::ScanPoint> scan_points =
427 data.getMeasurementDataPtr()->getScanPointsVector();
428 uint32_t num_scan_points = scan_points.size();
432 scan.angle_max = scan_points.empty()
435 scan.angle_increment =
sick::degToRad(
data.getDerivedValuesPtr()->getAngularBeamResolution());
436 boost::posix_time::microseconds time_increment =
437 boost::posix_time::microseconds(
data.getDerivedValuesPtr()->getInterbeamPeriod());
438 scan.time_increment = time_increment.total_microseconds() * 1e-6;
439 boost::posix_time::milliseconds scan_time =
440 boost::posix_time::milliseconds(
data.getDerivedValuesPtr()->getScanTime());
441 scan.scan_time = scan_time.total_microseconds() * 1e-6;
444 scan.ranges.resize(num_scan_points);
445 scan.intensities.resize(num_scan_points);
448 for (uint32_t i = 0; i < num_scan_points; ++i)
454 scan.ranges[i] =
static_cast<float>(scan_point.
getDistance()) *
455 data.getDerivedValuesPtr()->getMultiplicationFactor() * 1e-3;
460 scan.ranges[i] = std::numeric_limits<double>::infinity();
465 scan.ranges[i] = std::numeric_limits<double>::infinity();
467 scan.intensities[i] =
static_cast<float>(scan_point.
getReflectivity());
472 sick_safetyscanners::OutputPathsMsg
475 sick_safetyscanners::OutputPathsMsg
msg;
477 std::shared_ptr<sick::datastructure::ApplicationData> app_data =
data.getApplicationDataPtr();
488 if (monitoring_case_number_flags.size() > 0)
490 msg.active_monitoring_case = monitoring_case_numbers.at(0);
494 msg.active_monitoring_case = 0;
497 for (
size_t i = 0; i < eval_out.size(); i++)
499 msg.status.push_back(eval_out.at(i));
500 msg.is_safe.push_back(eval_out_is_safe.at(i));
501 msg.is_valid.push_back(eval_out_valid.at(i));
506 sick_safetyscanners::RawMicroScanDataMsg
509 sick_safetyscanners::RawMicroScanDataMsg
msg;
521 sick_safetyscanners::DataHeaderMsg
524 sick_safetyscanners::DataHeaderMsg
msg;
526 if (!
data.getDataHeaderPtr()->isEmpty())
528 std::shared_ptr<sick::datastructure::DataHeader> data_header =
data.getDataHeaderPtr();
530 msg.version_version = data_header->getVersionIndicator();
531 msg.version_release = data_header->getVersionRelease();
532 msg.version_major_version = data_header->getVersionMajorVersion();
533 msg.version_minor_version = data_header->getVersionMinorVersion();
535 msg.scan_number = data_header->getScanNumber();
536 msg.sequence_number = data_header->getSequenceNumber();
538 msg.serial_number_of_device = data_header->getSerialNumberOfDevice();
539 msg.serial_number_of_channel_plug = data_header->getSerialNumberOfSystemPlug();
541 msg.channel_number = data_header->getChannelNumber();
543 msg.timestamp_date = data_header->getTimestampDate();
544 msg.timestamp_time = data_header->getTimestampTime();
549 sick_safetyscanners::DerivedValuesMsg
552 sick_safetyscanners::DerivedValuesMsg
msg;
554 if (!
data.getDerivedValuesPtr()->isEmpty())
556 std::shared_ptr<sick::datastructure::DerivedValues> derived_values =
data.getDerivedValuesPtr();
558 msg.multiplication_factor = derived_values->getMultiplicationFactor();
559 msg.scan_time = derived_values->getScanTime();
560 msg.interbeam_period = derived_values->getInterbeamPeriod();
561 msg.number_of_beams = derived_values->getNumberOfBeams();
563 msg.angular_beam_resolution = derived_values->getAngularBeamResolution();
568 sick_safetyscanners::GeneralSystemStateMsg
571 sick_safetyscanners::GeneralSystemStateMsg
msg;
573 if (!
data.getGeneralSystemStatePtr()->isEmpty())
575 std::shared_ptr<sick::datastructure::GeneralSystemState> general_system_state =
576 data.getGeneralSystemStatePtr();
578 msg.run_mode_active = general_system_state->getRunModeActive();
579 msg.standby_mode_active = general_system_state->getStandbyModeActive();
580 msg.contamination_warning = general_system_state->getContaminationWarning();
581 msg.contamination_error = general_system_state->getContaminationError();
582 msg.reference_contour_status = general_system_state->getReferenceContourStatus();
583 msg.manipulation_status = general_system_state->getManipulationStatus();
585 std::vector<bool> safe_cut_off_path = general_system_state->getSafeCutOffPathVector();
586 for (
size_t i = 0; i < safe_cut_off_path.size(); i++)
588 msg.safe_cut_off_path.push_back(safe_cut_off_path.at(i));
591 std::vector<bool> non_safe_cut_off_path = general_system_state->getNonSafeCutOffPathVector();
592 for (
size_t i = 0; i < non_safe_cut_off_path.size(); i++)
594 msg.non_safe_cut_off_path.push_back(non_safe_cut_off_path.at(i));
597 std::vector<bool> reset_required_cut_off_path =
598 general_system_state->getResetRequiredCutOffPathVector();
599 for (
size_t i = 0; i < reset_required_cut_off_path.size(); i++)
601 msg.reset_required_cut_off_path.push_back(reset_required_cut_off_path.at(i));
604 msg.current_monitoring_case_no_table_1 =
605 general_system_state->getCurrentMonitoringCaseNoTable1();
606 msg.current_monitoring_case_no_table_2 =
607 general_system_state->getCurrentMonitoringCaseNoTable2();
608 msg.current_monitoring_case_no_table_3 =
609 general_system_state->getCurrentMonitoringCaseNoTable3();
610 msg.current_monitoring_case_no_table_4 =
611 general_system_state->getCurrentMonitoringCaseNoTable4();
613 msg.application_error = general_system_state->getApplicationError();
614 msg.device_error = general_system_state->getDeviceError();
619 sick_safetyscanners::MeasurementDataMsg
622 sick_safetyscanners::MeasurementDataMsg
msg;
624 if (!
data.getMeasurementDataPtr()->isEmpty())
626 msg.number_of_beams =
data.getMeasurementDataPtr()->getNumberOfBeams();
632 std::vector<sick_safetyscanners::ScanPointMsg>
635 std::vector<sick_safetyscanners::ScanPointMsg> msg_vector;
637 std::shared_ptr<sick::datastructure::MeasurementData> measurement_data =
638 data.getMeasurementDataPtr();
639 std::vector<sick::datastructure::ScanPoint> scan_points = measurement_data->getScanPointsVector();
641 uint32_t num_points = scan_points.size();
642 for (uint32_t i = 0; i < num_points; i++)
645 sick_safetyscanners::ScanPointMsg
msg;
656 msg_vector.push_back(
msg);
661 sick_safetyscanners::IntrusionDataMsg
664 sick_safetyscanners::IntrusionDataMsg
msg;
666 if (!
data.getIntrusionDataPtr()->isEmpty())
673 std::vector<sick_safetyscanners::IntrusionDatumMsg>
676 std::vector<sick_safetyscanners::IntrusionDatumMsg> msg_vector;
678 std::shared_ptr<sick::datastructure::IntrusionData> intrusion_data =
data.getIntrusionDataPtr();
679 std::vector<sick::datastructure::IntrusionDatum> intrusion_datums =
680 intrusion_data->getIntrusionDataVector();
682 for (
size_t i = 0; i < intrusion_datums.size(); i++)
684 sick_safetyscanners::IntrusionDatumMsg
msg;
688 for (
size_t j = 0; j < flags.size(); j++)
690 msg.flags.push_back(flags.at(j));
692 msg_vector.push_back(
msg);
697 sick_safetyscanners::ApplicationDataMsg
700 sick_safetyscanners::ApplicationDataMsg
msg;
702 if (!
data.getApplicationDataPtr()->isEmpty())
710 sick_safetyscanners::ApplicationInputsMsg
713 sick_safetyscanners::ApplicationInputsMsg
msg;
715 std::shared_ptr<sick::datastructure::ApplicationData> app_data =
data.getApplicationDataPtr();
719 for (
size_t i = 0; i < unsafe_inputs.size(); i++)
721 msg.unsafe_inputs_input_sources.push_back(unsafe_inputs.at(i));
722 msg.unsafe_inputs_flags.push_back(unsafe_inputs_flags.at(i));
726 for (
size_t i = 0; i < monitoring_case.size(); i++)
728 msg.monitoring_case_number_inputs.push_back(monitoring_case.at(i));
729 msg.monitoring_case_number_inputs_flags.push_back(monitoring_case_flags.at(i));
743 sick_safetyscanners::ApplicationOutputsMsg
746 sick_safetyscanners::ApplicationOutputsMsg
msg;
748 std::shared_ptr<sick::datastructure::ApplicationData> app_data =
data.getApplicationDataPtr();
754 for (
size_t i = 0; i < eval_out.size(); i++)
756 msg.evaluation_path_outputs_eval_out.push_back(eval_out.at(i));
757 msg.evaluation_path_outputs_is_safe.push_back(eval_out_is_safe.at(i));
758 msg.evaluation_path_outputs_is_valid.push_back(eval_out_valid.at(i));
763 for (
size_t i = 0; i < monitoring_case.size(); i++)
765 msg.monitoring_case_number_outputs.push_back(monitoring_case.at(i));
766 msg.monitoring_case_number_outputs_flags.push_back(monitoring_case_flags.at(i));
781 msg.linear_velocity_outputs_velocity_0_transmitted_safely =
785 msg.linear_velocity_outputs_velocity_1_transmitted_safely =
792 for (
size_t i = 0; i < resulting_velocities.size(); i++)
794 msg.resulting_velocity.push_back(resulting_velocities.at(i));
795 msg.resulting_velocity_flags.push_back(resulting_velocities_flags.at(i));
803 sick_safetyscanners::FieldData::Response& res)
805 std::vector<sick::datastructure::FieldData> fields;
808 for (
size_t i = 0; i < fields.size(); i++)
811 sick_safetyscanners::FieldMsg field_msg;
818 for (
size_t j = 0; j < ranges.size(); j++)
820 field_msg.ranges.push_back(
static_cast<float>(ranges.at(j)) * 1e-3);
823 res.fields.push_back(field_msg);
831 std::vector<sick::datastructure::MonitoringCaseData> monitoring_cases;
834 for (
size_t i = 0; i < monitoring_cases.size(); i++)
837 sick_safetyscanners::MonitoringCaseMsg monitoring_case_msg;
840 std::vector<uint16_t> mon_fields = monitoring_case_data.
getFieldIndices();
841 std::vector<bool> mon_fields_valid = monitoring_case_data.
getFieldsValid();
842 for (
size_t j = 0; j < mon_fields.size(); j++)
844 monitoring_case_msg.fields.push_back(mon_fields.at(j));
845 monitoring_case_msg.fields_valid.push_back(mon_fields_valid.at(j));
847 res.monitoring_cases.push_back(monitoring_case_msg);
854 sick_safetyscanners::ConfigMetadata::Response& res)
856 static_cast<void>(req);
861 res.integrity_hash.clear();
863 for (
const auto& hash : integrity_hash)
865 res.integrity_hash.push_back(hash);
869 res.modification_time =
getDateString(res.modification_time_date, res.modification_time_time);
873 res.transfer_time =
getDateString(res.transfer_time_date, res.transfer_time_time);
883 sick_safetyscanners::StatusOverview::Response& res)
885 static_cast<void>(req);
902 res.current_time =
getDateString(res.current_time_date, res.current_time_time);
908 res.error_info_time =
getDateString(res.error_info_time_date, res.error_info_time_time);
915 std::stringstream ss;
916 ss <<
"0x" << std::hex << (checksum & 0xFF) << ((checksum & 0xFF00) >> 8)
917 << ((checksum & 0xFF0000) >> 16) << ((checksum & 0xFF000000) >> 24);
923 std::time_t t =
static_cast<std::time_t
>((730
926 milli_seconds * 0.001);
929 strftime(buffer, 40,
"%F %X", gmtime(&t));