diff --git a/.cspell.json b/.cspell.json index f48e528d0..553200968 100644 --- a/.cspell.json +++ b/.cspell.json @@ -28,6 +28,7 @@ "horiz", "Idat", "ipaddr", + "libmsgsl", "manc", "mdeg", "memcpy", diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index d4de78afa..2334e2794 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -26,6 +26,7 @@ find_package(sensor_msgs REQUIRED) find_package(velodyne_msgs REQUIRED) find_package(yaml-cpp REQUIRED) find_package(PNG REQUIRED) +find_package(Microsoft.GSL REQUIRED) # Check if pcl_conversions provides modern CMake targets (ROS 2 Jazzy and later) # or if we need to use legacy variables (ROS 2 Humble and earlier) @@ -64,6 +65,7 @@ add_library(nebula_decoders_hesai SHARED target_link_libraries(nebula_decoders_hesai PUBLIC ${pandar_msgs_TARGETS} ${PNG_LIBRARIES} + Microsoft.GSL::GSL ) target_include_directories(nebula_decoders_hesai PUBLIC @@ -177,6 +179,7 @@ ament_export_dependencies( sensor_msgs velodyne_msgs yaml-cpp + Microsoft.GSL ) ament_package() diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index 2f387da0f..9d630dc9e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -23,6 +23,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/packet_loss_detector.hpp" +#include #include #include #include @@ -106,7 +107,7 @@ class HesaiDecoder : public HesaiScanDecoder /// @brief Validates and parse PandarPacket. Checks size and, if present, CRC checksums. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parse_packet(const std::vector & packet) + bool parse_packet(gsl::span packet) { if (packet.size() < sizeof(typename SensorT::packet_t)) { NEBULA_LOG_STREAM( @@ -350,7 +351,7 @@ class HesaiDecoder : public HesaiScanDecoder pointcloud_callback_ = std::move(callback); } - PacketDecodeResult unpack(const std::vector & packet) override + PacketDecodeResult unpack(gsl::span packet) override { util::Stopwatch decode_watch; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index 50ec7e41b..5cb6c4440 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -15,6 +15,7 @@ #ifndef NEBULA_WS_HESAI_SCAN_DECODER_HPP #define NEBULA_WS_HESAI_SCAN_DECODER_HPP +#include #include #include #include @@ -74,7 +75,7 @@ class HesaiScanDecoder /// @param packet The incoming PandarPacket /// @return Metadata on success, or decode error on failure. Performance counters are always /// returned. - virtual PacketDecodeResult unpack(const std::vector & packet) = 0; + virtual PacketDecodeResult unpack(gsl::span packet) = 0; virtual void set_pointcloud_callback(pointcloud_callback_t callback) = 0; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index e3a144864..87b6f91ee 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -24,6 +24,7 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/packet_loss_detector.hpp" +#include #include #include @@ -140,7 +141,7 @@ class HesaiDriver /// @param packet Packet to decode /// @return Metadata on success, or decode error on failure. Performance counters are always /// returned. - PacketDecodeResult parse_cloud_packet(const std::vector & packet); + PacketDecodeResult parse_cloud_packet(gsl::span packet); }; } // namespace nebula::drivers diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index 4e01836ff..3a6fc19c2 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -21,6 +21,7 @@ angles continental_msgs diagnostic_msgs + libmsgsl-dev nebula_common nebula_msgs pandar_msgs diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 19718f26e..faf6ac777 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -16,6 +16,8 @@ #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp" +#include + #include #include #include @@ -116,7 +118,7 @@ std::shared_ptr HesaiDriver::initialize_decoder( std::move(blockage_mask_plugin)); } -PacketDecodeResult HesaiDriver::parse_cloud_packet(const std::vector & packet) +PacketDecodeResult HesaiDriver::parse_cloud_packet(gsl::span packet) { if (driver_status_ != nebula::Status::OK) { logger_->error("Driver not OK."); diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 2c7528d36..d605872c2 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -34,6 +34,7 @@ find_package(message_filters REQUIRED) find_package(autoware_utils_debug REQUIRED) find_package(autoware_internal_debug_msgs REQUIRED) find_package(agnocastlib) +find_package(Microsoft.GSL REQUIRED) if("$ENV{ENABLE_AGNOCAST}" AND NOT agnocastlib_FOUND) message(FATAL_ERROR "agnocastlib is required when Agnocast is enabled") @@ -82,6 +83,7 @@ target_link_libraries(hesai_ros_wrapper PUBLIC ${autoware_internal_debug_msgs_TARGETS} nebula_decoders::nebula_decoders_hesai nebula_hw_interfaces::nebula_hw_interfaces_hesai + Microsoft.GSL::GSL ) if("$ENV{ENABLE_AGNOCAST}") @@ -331,6 +333,7 @@ ament_export_dependencies( sync_tooling_msgs autoware_utils_debug autoware_internal_debug_msgs + Microsoft.GSL ) if("$ENV{ENABLE_AGNOCAST}") diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index 60dbeb1d3..3eccfb78b 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -25,12 +25,12 @@ #include #include #include +#include #include #include #include #include -#include #include #include @@ -51,11 +51,12 @@ class HesaiDecoderWrapper diagnostic_updater::Updater & diagnostic_updater, bool publish_packets); /// @brief Process a cloud packet and return metadata - /// @param packet_msg The packet to process - /// @param receive_metadata Performance metadata from packet reception + /// @param packet_data The raw packet data + /// @param packet_stamp_ns Timestamp of the packet in nanoseconds + /// @param receive_time_ns Performance metadata from packet reception /// @return Expected containing metadata on success, or decode error on failure drivers::PacketDecodeResult process_cloud_packet( - std::unique_ptr packet_msg, uint64_t receive_time_ns); + gsl::span packet_data, uint64_t packet_stamp_ns, uint64_t receive_time_ns); void on_pointcloud_decoded(const drivers::NebulaPointCloudPtr & pointcloud, double timestamp_s); diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index c0c8830fc..0346635be 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -28,7 +28,6 @@ #include #include -#include #include #include diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 4d5d58595..b4402a7a0 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -24,6 +24,7 @@ diagnostic_msgs diagnostic_updater geometry_msgs + libmsgsl-dev message_filters nebula_common nebula_decoders diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 01f7ba2e3..d0cda6a40 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -9,6 +9,8 @@ #include "nebula_ros/hesai/diagnostics/functional_safety_basic.hpp" #include "nebula_ros/hesai/diagnostics/functional_safety_diagnostic_task.hpp" +#include +#include #include #include #include @@ -111,7 +113,7 @@ void HesaiDecoderWrapper::on_calibration_change( } drivers::PacketDecodeResult HesaiDecoderWrapper::process_cloud_packet( - std::unique_ptr packet_msg, uint64_t receive_time_ns) + gsl::span packet_data, uint64_t packet_stamp_ns, uint64_t receive_time_ns) { current_scan_perf_counters_.receive_time_current_scan_ns += receive_time_ns; @@ -120,19 +122,20 @@ drivers::PacketDecodeResult HesaiDecoderWrapper::process_cloud_packet( // publish thread, negating the benefits of multi-threading. // Not checking is an okay trade-off for the time being. if (packets_pub_) { + rclcpp::Time packet_stamp(static_cast(packet_stamp_ns)); if (current_scan_msg_->packets.size() == 0) { - current_scan_msg_->header.stamp = packet_msg->stamp; + current_scan_msg_->header.stamp = packet_stamp; } pandar_msgs::msg::PandarPacket pandar_packet_msg{}; - pandar_packet_msg.stamp = packet_msg->stamp; - pandar_packet_msg.size = packet_msg->data.size(); - std::copy(packet_msg->data.begin(), packet_msg->data.end(), pandar_packet_msg.data.begin()); + pandar_packet_msg.stamp = packet_stamp; + pandar_packet_msg.size = packet_data.size(); + std::copy(packet_data.begin(), packet_data.end(), pandar_packet_msg.data.begin()); current_scan_msg_->packets.emplace_back(pandar_packet_msg); } std::lock_guard lock(mtx_driver_ptr_); - auto decode_result = driver_ptr_->parse_cloud_packet(packet_msg->data); + auto decode_result = driver_ptr_->parse_cloud_packet(packet_data); current_scan_perf_counters_.decode_time_current_scan_ns += decode_result.performance_counters.decode_time_ns; diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index c00d44580..5ec43da3a 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -4,6 +4,7 @@ #include "nebula_ros/common/parameter_descriptors.hpp" +#include #include #include #include @@ -414,12 +415,13 @@ void HesaiRosWrapper::receive_scan_message_callback( return; } - for (auto & pkt : scan_msg->packets) { - auto nebula_pkt_ptr = std::make_unique(); - nebula_pkt_ptr->stamp = pkt.stamp; - std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + for (const auto & pkt : scan_msg->packets) { + uint64_t packet_stamp_ns = static_cast(pkt.stamp.sec) * 1'000'000'000ULL + + static_cast(pkt.stamp.nanosec); + gsl::span packet_data(pkt.data.data(), pkt.size); - decoder_wrapper_->process_cloud_packet(std::move(nebula_pkt_ptr), receive_watch.elapsed_ns()); + decoder_wrapper_->process_cloud_packet( + packet_data, packet_stamp_ns, receive_watch.elapsed_ns()); // This reset is placed at the end of the loop, so that in the first iteration, the possible // logging overhead from the statements before the loop is included in the measurement. receive_watch.reset(); @@ -578,13 +580,10 @@ void HesaiRosWrapper::receive_cloud_packet_callback( const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto msg_ptr = std::make_unique(); - msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); - msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); - msg_ptr->data = packet; + gsl::span packet_data(packet); auto decode_result = decoder_wrapper_->process_cloud_packet( - std::move(msg_ptr), receive_metadata.packet_perf_counters.receive_duration_ns); + packet_data, timestamp_ns, receive_metadata.packet_perf_counters.receive_duration_ns); if ( decode_result.metadata_or_error.has_value() && sync_tooling_plugin_ &&