From 20b413016b897083dc1b6f04216af15512ab8abd Mon Sep 17 00:00:00 2001 From: Finn Weithoff Date: Fri, 12 Apr 2024 13:57:48 +0200 Subject: [PATCH] Optionally publish output_paths with a header --- CMakeLists.txt | 1 + README.md | 1 + .../SickSafetyscannersRos.h | 3 ++ launch/sick_safetyscanners.launch | 4 ++- msg/OutputPathsStampedMsg.msg | 2 ++ src/SickSafetyscannersRos.cpp | 32 +++++++++++++++++-- 6 files changed, 39 insertions(+), 4 deletions(-) create mode 100644 msg/OutputPathsStampedMsg.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 8c3f423..c51de06 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ add_message_files( MeasurementDataMsg.msg MonitoringCaseMsg.msg OutputPathsMsg.msg + OutputPathsStampedMsg.msg RawMicroScanDataMsg.msg ScanPointMsg.msg ) diff --git a/README.md b/README.md index 5209233..46d4bb7 100644 --- a/README.md +++ b/README.md @@ -142,6 +142,7 @@ All Parameters can be passed as commandline argument to the launch file. | ------------- | ------ | ------- | ------------ | ------------- | | sensor_ip | String | 192.168.1.11 | ✔ | IP address of the sensor. | | host_ip | String | 192.168.1.9 | ✔ | IP address of the receiving host/target computer. | +| output_paths_header | Boolean | False | ✔ | If OutputPathsStampedMsg should be used instead of OutputPathsMsg. | | interface_ip | String | 0.0.0.0 | | Interface IP address of the receiving host computer, this needs to be set if the host IP is in the multicast IP range. The default is an undefined IP address and will return an error when multicast is used without a correct interface | | host_udp_port | Integer | 0 | | Host UDP Port. Zero allows system chosen port. | | frame_id | String | scan | | The frame name of the sensor message | diff --git a/include/sick_safetyscanners/SickSafetyscannersRos.h b/include/sick_safetyscanners/SickSafetyscannersRos.h index 80734c5..73e71d6 100644 --- a/include/sick_safetyscanners/SickSafetyscannersRos.h +++ b/include/sick_safetyscanners/SickSafetyscannersRos.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -157,6 +158,8 @@ class SickSafetyscannersRos dynamic_reconfigure::Server m_dynamic_reconfiguration_server; + bool m_output_paths_header; + std::string m_frame_id; double m_time_offset; double m_range_min; diff --git a/launch/sick_safetyscanners.launch b/launch/sick_safetyscanners.launch index cff7cbd..4179d5a 100644 --- a/launch/sick_safetyscanners.launch +++ b/launch/sick_safetyscanners.launch @@ -16,7 +16,8 @@ - + + @@ -38,6 +39,7 @@ + diff --git a/msg/OutputPathsStampedMsg.msg b/msg/OutputPathsStampedMsg.msg new file mode 100644 index 0000000..15fb728 --- /dev/null +++ b/msg/OutputPathsStampedMsg.msg @@ -0,0 +1,2 @@ +Header header +OutputPathsMsg output_paths \ No newline at end of file diff --git a/src/SickSafetyscannersRos.cpp b/src/SickSafetyscannersRos.cpp index 5eb2ec8..70228bb 100644 --- a/src/SickSafetyscannersRos.cpp +++ b/src/SickSafetyscannersRos.cpp @@ -62,8 +62,16 @@ SickSafetyscannersRos::SickSafetyscannersRos() m_extended_laser_scan_publisher = m_nh.advertise("extended_laser_scan", 100); m_raw_data_publisher = m_nh.advertise("raw_data", 100); - m_output_path_publisher = - m_nh.advertise("output_paths", 100); + if (m_output_paths_header) + { + m_output_path_publisher = + m_nh.advertise("output_paths", 100); + } + else + { + m_output_path_publisher = + m_nh.advertise("output_paths", 100); + } m_field_service_server = m_nh.advertiseService("field_data", &SickSafetyscannersRos::getFieldData, this); @@ -196,6 +204,12 @@ bool SickSafetyscannersRos::readParameters() } m_communication_settings.setHostUdpPort(host_udp_port); + m_output_paths_header = false; + if (!m_private_nh.getParam("output_paths_header", m_output_paths_header)) + { + ROS_WARN("Using default OutputPathsMsg (without header)."); + } + ROS_WARN("If not further specified the default values for the dynamic reconfigurable parameters " "will be loaded."); @@ -281,7 +295,19 @@ void SickSafetyscannersRos::receivedUDPPacket(const sick::datastructure::Data& d m_extended_laser_scan_publisher.publish(extended_scan); sick_safetyscanners::OutputPathsMsg output_paths = createOutputPathsMessage(data); - m_output_path_publisher.publish(output_paths); + if (m_output_paths_header) + { + sick_safetyscanners::OutputPathsStampedMsg stamped_output_paths; + stamped_output_paths.header.seq = extended_scan.laser_scan.header.seq; + stamped_output_paths.header.stamp = extended_scan.laser_scan.header.stamp; + stamped_output_paths.header.frame_id = extended_scan.laser_scan.header.frame_id; + stamped_output_paths.output_paths = output_paths; + m_output_path_publisher.publish(stamped_output_paths); + } + else + { + m_output_path_publisher.publish(output_paths); + } } m_last_raw_data = createRawDataMessage(data);