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);
81 m_device = std::make_shared<sick::SickSafetyscanners>(
93 m_device->changeSensorSettings(m_communication_settings);
95 ROS_INFO(
"Successfully launched node.");
100 ROS_INFO(
"Reading Type code settings");
110 ROS_INFO(
"Reading Persistent Configuration");
118 const sick_safetyscanners::SickSafetyscannersConfigurationConfig& config,
const uint32_t& level)
124 if (config.angle_start == config.angle_end)
135 config.derived_settings,
136 config.measurement_data,
137 config.intrusion_data,
138 config.application_io_data);
161 std::string sensor_ip_adress =
"192.168.1.10";
165 ROS_WARN(
"Using default sensor IP: %s", sensor_ip_adress.c_str());
170 std::string host_ip_adress =
"192.168.1.9";
173 ROS_WARN(
"Using default host IP: %s", host_ip_adress.c_str());
177 std::string interface_ip_adress =
"0.0.0.0";
180 ROS_WARN(
"Using default interface IP: %s", interface_ip_adress.c_str());
182 m_interface_ip = boost::asio::ip::address_v4::from_string(interface_ip_adress);
184 int host_udp_port = 0;
187 ROS_WARN(
"Using default host UDP Port: %i", host_udp_port);
191 ROS_WARN(
"If not further specified the default values for the dynamic reconfigurable parameters " 219 if (angle_start == angle_end)
232 bool general_system_state;
235 bool derived_settings;
238 bool measurement_data;
244 bool application_io_data;
248 general_system_state, derived_settings, measurement_data, intrusion_data, application_io_data);
287 return b ?
"true" :
"false";
293 const sick_safetyscanners::DataHeaderMsg& header =
m_last_raw_data.header;
294 if (header.timestamp_time == 0 && header.timestamp_date == 0)
296 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::STALE,
297 "Could not get sensor state");
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);
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));
332 if (state.device_error)
334 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"Device error");
338 diagnostic_status.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"OK");
342 sick_safetyscanners::ExtendedLaserScanMsg
346 sick_safetyscanners::ExtendedLaserScanMsg
msg;
347 msg.laser_scan = scan;
349 std::vector<sick::datastructure::ScanPoint> scan_points =
351 uint32_t num_scan_points = scan_points.size();
354 msg.reflektor_status.resize(num_scan_points);
355 msg.intrusion.resize(num_scan_points);
356 msg.reflektor_median.resize(num_scan_points);
358 for (uint32_t i = 0; i < num_scan_points; ++i)
363 msg.reflektor_median[i] = medians.at(i);
369 const std::vector<sick::datastructure::ScanPoint> scan_points)
371 std::vector<bool> res;
372 res.resize(scan_points.size());
375 for (
size_t i = 0; i < scan_points.size(); i++)
378 if (!last && scan_points.at(i).getReflectorBit())
383 else if (last && (!scan_points.at(i).getReflectorBit() || i == scan_points.size() - 1))
386 res.at(start + ((i - start) / 2)) =
true;
393 sensor_msgs::LaserScan
396 sensor_msgs::LaserScan scan;
402 std::vector<sick::datastructure::ScanPoint> scan_points =
404 uint32_t num_scan_points = scan_points.size();
409 ->getScanPointsVector()
413 scan.angle_max = angle_max;
415 boost::posix_time::microseconds time_increment =
417 scan.time_increment = time_increment.total_microseconds() * 1e-6;
418 boost::posix_time::milliseconds scan_time =
420 scan.scan_time = scan_time.total_microseconds() * 1e-6;
423 scan.ranges.resize(num_scan_points);
424 scan.intensities.resize(num_scan_points);
427 for (uint32_t i = 0; i < num_scan_points; ++i)
433 scan.ranges[i] =
static_cast<float>(scan_point.
getDistance()) *
439 scan.ranges[i] = std::numeric_limits<double>::infinity();
444 scan.ranges[i] = std::numeric_limits<double>::infinity();
446 scan.intensities[i] =
static_cast<float>(scan_point.
getReflectivity());
451 sick_safetyscanners::OutputPathsMsg
454 sick_safetyscanners::OutputPathsMsg
msg;
467 if (monitoring_case_number_flags.size() > 0)
469 msg.active_monitoring_case = monitoring_case_numbers.at(0);
473 msg.active_monitoring_case = 0;
476 for (
size_t i = 0; i < eval_out.size(); i++)
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));
485 sick_safetyscanners::RawMicroScanDataMsg
488 sick_safetyscanners::RawMicroScanDataMsg
msg;
500 sick_safetyscanners::DataHeaderMsg
503 sick_safetyscanners::DataHeaderMsg
msg;
507 std::shared_ptr<sick::datastructure::DataHeader> data_header = data.
getDataHeaderPtr();
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();
514 msg.scan_number = data_header->getScanNumber();
515 msg.sequence_number = data_header->getSequenceNumber();
517 msg.serial_number_of_device = data_header->getSerialNumberOfDevice();
518 msg.serial_number_of_channel_plug = data_header->getSerialNumberOfSystemPlug();
520 msg.channel_number = data_header->getChannelNumber();
522 msg.timestamp_date = data_header->getTimestampDate();
523 msg.timestamp_time = data_header->getTimestampTime();
528 sick_safetyscanners::DerivedValuesMsg
531 sick_safetyscanners::DerivedValuesMsg
msg;
535 std::shared_ptr<sick::datastructure::DerivedValues> derived_values = data.
getDerivedValuesPtr();
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();
547 sick_safetyscanners::GeneralSystemStateMsg
550 sick_safetyscanners::GeneralSystemStateMsg
msg;
554 std::shared_ptr<sick::datastructure::GeneralSystemState> general_system_state =
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();
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++)
567 msg.safe_cut_off_path.push_back(safe_cut_off_path.at(i));
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++)
573 msg.non_safe_cut_off_path.push_back(non_safe_cut_off_path.at(i));
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++)
580 msg.reset_required_cut_off_path.push_back(reset_required_cut_off_path.at(i));
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();
592 msg.application_error = general_system_state->getApplicationError();
593 msg.device_error = general_system_state->getDeviceError();
598 sick_safetyscanners::MeasurementDataMsg
601 sick_safetyscanners::MeasurementDataMsg
msg;
611 std::vector<sick_safetyscanners::ScanPointMsg>
614 std::vector<sick_safetyscanners::ScanPointMsg> msg_vector;
616 std::shared_ptr<sick::datastructure::MeasurementData> measurement_data =
618 std::vector<sick::datastructure::ScanPoint> scan_points = measurement_data->getScanPointsVector();
620 uint32_t num_points = scan_points.size();
621 for (uint32_t i = 0; i < num_points; i++)
624 sick_safetyscanners::ScanPointMsg
msg;
635 msg_vector.push_back(msg);
640 sick_safetyscanners::IntrusionDataMsg
643 sick_safetyscanners::IntrusionDataMsg
msg;
652 std::vector<sick_safetyscanners::IntrusionDatumMsg>
655 std::vector<sick_safetyscanners::IntrusionDatumMsg> msg_vector;
657 std::shared_ptr<sick::datastructure::IntrusionData> intrusion_data = data.
getIntrusionDataPtr();
658 std::vector<sick::datastructure::IntrusionDatum> intrusion_datums =
659 intrusion_data->getIntrusionDataVector();
661 for (
size_t i = 0; i < intrusion_datums.size(); i++)
663 sick_safetyscanners::IntrusionDatumMsg
msg;
665 msg.size = intrusion_datum.
getSize();
667 for (
size_t j = 0; j < flags.size(); j++)
669 msg.flags.push_back(flags.at(j));
671 msg_vector.push_back(msg);
676 sick_safetyscanners::ApplicationDataMsg
679 sick_safetyscanners::ApplicationDataMsg
msg;
689 sick_safetyscanners::ApplicationInputsMsg
692 sick_safetyscanners::ApplicationInputsMsg
msg;
698 for (
size_t i = 0; i < unsafe_inputs.size(); i++)
700 msg.unsafe_inputs_input_sources.push_back(unsafe_inputs.at(i));
701 msg.unsafe_inputs_flags.push_back(unsafe_inputs_flags.at(i));
705 for (
size_t i = 0; i < monitoring_case.size(); i++)
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));
710 msg.linear_velocity_inputs_velocity_0 = inputs.
getVelocity0();
713 msg.linear_velocity_inputs_velocity_1 = inputs.
getVelocity1();
722 sick_safetyscanners::ApplicationOutputsMsg
725 sick_safetyscanners::ApplicationOutputsMsg
msg;
733 for (
size_t i = 0; i < eval_out.size(); i++)
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));
742 for (
size_t i = 0; i < monitoring_case.size(); i++)
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));
759 msg.linear_velocity_outputs_velocity_0 = outputs.
getVelocity0();
760 msg.linear_velocity_outputs_velocity_0_transmitted_safely =
763 msg.linear_velocity_outputs_velocity_1 = outputs.
getVelocity1();
764 msg.linear_velocity_outputs_velocity_1_transmitted_safely =
771 for (
size_t i = 0; i < resulting_velocities.size(); i++)
773 msg.resulting_velocity.push_back(resulting_velocities.at(i));
774 msg.resulting_velocity_flags.push_back(resulting_velocities_flags.at(i));
782 sick_safetyscanners::FieldData::Response& res)
784 std::vector<sick::datastructure::FieldData> fields;
787 for (
size_t i = 0; i < fields.size(); i++)
790 sick_safetyscanners::FieldMsg field_msg;
797 for (
size_t j = 0; j < ranges.size(); j++)
799 field_msg.ranges.push_back(static_cast<float>(ranges.at(j)) * 1e-3);
802 res.fields.push_back(field_msg);
810 std::vector<sick::datastructure::MonitoringCaseData> monitoring_cases;
813 for (
size_t i = 0; i < monitoring_cases.size(); i++)
816 sick_safetyscanners::MonitoringCaseMsg monitoring_case_msg;
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++)
823 monitoring_case_msg.fields.push_back(mon_fields.at(j));
824 monitoring_case_msg.fields_valid.push_back(mon_fields_valid.at(j));
826 res.monitoring_cases.push_back(monitoring_case_msg);
bool getVelocity1TransmittedSafely() const
Gets if the second linear velocity output is transmitted safely.
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.
sick_safetyscanners::MeasurementDataMsg createMeasurementDataMessage(const sick::datastructure::Data &data)
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.
bool getHostErrorFlagContaminationError() const
Gets if a contamination error is present.
bool getFlagsSleepModeOutputIsValid() const
Gets if the sleep mode is valid.
void publish(const boost::shared_ptr< M > &message) const
bool getInfiniteBit() const
Returns if the scanpoint is infinite.
void setSensorTcpPort(const uint16_t &sensor_tcp_port)
Sets the sensor tcp port.
int32_t getSize() const
Returns size of flag vector.
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.
bool getIsProtectiveField() const
Returns if a field is a protective field.
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.
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.
std::vector< uint16_t > getMonitoringCaseVector() const
Gets the currently active monitoring case numbers.
Class containing the data of a single scan point.
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.
Field data for warning and protective fields.
void sensorDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &diagnostic_status)
double m_frequency_tolerance
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.
void setPublishingFrequency(const uint16_t &publishing_frequency)
Sets the publishing frequency.
int16_t getVelocity0() const
Gets the first linear velocity output.
uint16_t getDistance() const
Getter for the distance of the scanpoint.
std::vector< bool > getFlagsVector() const
Getter for the flags vector.
sick_safetyscanners::ApplicationDataMsg createApplicationDataMessage(const sick::datastructure::Data &data)
void addf(const std::string &key, const char *format,...)
std::shared_ptr< IntrusionData > getIntrusionDataPtr() const
Gets the intrusion data.
The data class containing all data blocks of a measurement.
dynamic_reconfigure::Server< sick_safetyscanners::SickSafetyscannersConfigurationConfig > m_dynamic_reconfiguration_server
bool getGlareBit() const
Returns if the scanpoint has glare.
bool getValidBit() const
Returns if the scanpoint is valid.
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.
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.
bool getFieldData(sick_safetyscanners::FieldData::Request &req, sick_safetyscanners::FieldData::Response &res)
bool getFlagsHostErrorFlagsAreValid() const
Gets if the error flags are valid.
ros::Publisher m_output_path_publisher
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.
std::string getDeviceName() const
Gets the device name for the scanner.
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.
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.
sick::datastructure::CommSettings m_communication_settings
The application outputs from a udp data packet.
Config data for current and persistent sensor config.
void setHostUdpPort(const uint16_t &host_udp_port)
Sets the host udp port.
double m_expected_frequency
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.
ros::Publisher m_raw_data_publisher
float radToDeg(float rad)
Converts radians to degrees.
sick_safetyscanners::RawMicroScanDataMsg m_last_raw_data
boost::asio::ip::address_v4 m_interface_ip
double m_timestamp_max_acceptable
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.
void readTypeCodeSettings()
Class containing the type code of a laser scanner.
std::shared_ptr< GeneralSystemState > getGeneralSystemStatePtr() const
Gets the general system state.
std::shared_ptr< DataHeader > getDataHeaderPtr() const
Gets the data header.
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.
float getEndAngle() const
Gets the end angle of the scan.
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.
void readPersistentConfig()
double m_timestamp_min_acceptable
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.
uint16_t getMonitoringCaseNumber() const
Returns the number of the monitoring case.
std::shared_ptr< DerivedValues > getDerivedValuesPtr() const
Gets the derived values.
float getAngularBeamResolution() const
Returns the angular resolution between the beams.
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.