From f99755538dbb1c05bb7e1e167d2bcc67dfcd8739 Mon Sep 17 00:00:00 2001 From: Maximilian Schmeller Date: Thu, 21 Mar 2024 18:34:02 +0900 Subject: [PATCH] refactor: remove pandarScan pub/sub, decode one packet at a time --- .../decoders/hesai_decoder.hpp | 14 +++--- .../decoders/hesai_scan_decoder.hpp | 4 +- .../nebula_decoders_hesai/hesai_driver.hpp | 4 +- .../nebula_decoders_hesai/hesai_driver.cpp | 25 +++++----- .../hesai_hw_interface.hpp | 6 +-- .../hesai_hw_interface.cpp | 48 ++----------------- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 6 +-- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 29 +++++------ 8 files changed, 41 insertions(+), 95 deletions(-) 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 b81b45d44..8f28856e8 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 @@ -52,17 +52,17 @@ class HesaiDecoder : public HesaiScanDecoder block_firing_offset_ns_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet) + bool parsePacket(const std::vector & packet) { - if (pandar_packet.size < sizeof(typename SensorT::packet_t)) { + if (packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( - logger_, "Packet size mismatch:" << pandar_packet.size << " | Expected at least:" + logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:" << sizeof(typename SensorT::packet_t)); return false; } - if (std::memcpy(&packet_, pandar_packet.data.data(), sizeof(typename SensorT::packet_t))) { + if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { // FIXME(mojomex) do validation? // RCLCPP_DEBUG(logger_, "Packet parsed successfully"); return true; @@ -222,9 +222,9 @@ class HesaiDecoder : public HesaiScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } - int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override + int unpack(const std::vector & packet) override { - if (!parsePacket(pandar_packet)) { + if (!parsePacket(packet)) { return -1; } 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 f0b4e3f8b..9d06c2702 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 @@ -26,9 +26,9 @@ class HesaiScanDecoder HesaiScanDecoder() = default; /// @brief Parses PandarPacket and add its points to the point cloud - /// @param pandar_packet The incoming PandarPacket + /// @param packet The incoming PandarPacket /// @return The last azimuth processed - virtual int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0; + virtual int unpack(const std::vector & packet) = 0; /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready 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 c83788965..a3f37cd32 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 @@ -54,8 +54,8 @@ class HesaiDriver : NebulaDriverBase /// @brief Convert PandarScan message to point cloud /// @param pandar_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan); + std::tuple ParseCloudPacket( + const std::vector & packet); }; } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 49be4adac..61b969ed7 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -72,8 +72,8 @@ HesaiDriver::HesaiDriver( } } -std::tuple HesaiDriver::ConvertScanToPointcloud( - const std::shared_ptr & pandar_scan) +std::tuple HesaiDriver::ParseCloudPacket( + const std::vector & packet) { std::tuple pointcloud; auto logger = rclcpp::get_logger("HesaiDriver"); @@ -83,20 +83,17 @@ std::tuple HesaiDriver::ConvertScanToPoint return pointcloud; } - int cnt = 0, last_azimuth = 0; - for (auto & packet : pandar_scan->packets) { - last_azimuth = scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); - cnt++; - } + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); } - if (cnt == 0) { - RCLCPP_ERROR_STREAM( - logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " - << "pointclouds were generated. Last azimuth: " << last_azimuth); - } + // todo + // if (cnt == 0) { + // RCLCPP_ERROR_STREAM( + // logger, "Scanned " << pandar_scan->packets.size() << " packets, but no " + // << "pointclouds were generated. Last azimuth: " << last_azimuth); + // } return pointcloud; } diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 2f5639373..f65daf077 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -102,8 +102,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase std::unique_ptr scan_cloud_ptr_; std::function is_valid_packet_; /*Lambda Function Array to verify proper packet size*/ - std::function buffer)> - scan_reception_callback_; /**This function pointer is called when the scan is complete*/ + std::function & buffer)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ int prev_phase_{}; @@ -192,7 +192,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @param scan_callback Callback function /// @return Resulting status Status RegisterScanCallback( - std::function)> scan_callback); + std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string GetLidarCalibrationString(); diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index a5e5bd068..bf3060afc 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -218,61 +218,19 @@ Status HesaiHwInterface::SensorInterfaceStart() } Status HesaiHwInterface::RegisterScanCallback( - std::function)> scan_callback) + std::function &)> scan_callback) { - scan_reception_callback_ = std::move(scan_callback); + cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) { - int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); if (!is_valid_packet_(buffer.size())) { PrintDebug("Invalid Packet: " + std::to_string(buffer.size())); return; } - const uint32_t buffer_size = buffer.size(); - pandar_msgs::msg::PandarPacket pandar_packet; - std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, pandar_packet.data.begin()); - pandar_packet.size = buffer_size; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - pandar_packet.stamp.sec = static_cast(now_secs); - pandar_packet.stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); - scan_cloud_ptr_->packets.emplace_back(pandar_packet); - - int current_phase = 0; - bool comp_flg = false; - - const auto & data = scan_cloud_ptr_->packets.back().data; - current_phase = (data[azimuth_index_] & 0xff) + ((data[azimuth_index_ + 1] & 0xff) << 8); - if (is_solid_state) { - current_phase = (static_cast(current_phase) + 36000 - 0) % 12000; - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } else { - current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; - - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; - } else { - comp_flg = true; - } - } - - if (comp_flg) { // Scan complete - if (scan_reception_callback_) { - scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp; - // Callback - scan_reception_callback_(std::move(scan_cloud_ptr_)); - scan_cloud_ptr_ = std::make_unique(); - } - } + cloud_packet_callback_(buffer); } Status HesaiHwInterface::SensorInterfaceStop() { 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 f23d1f9c4..adce54b8f 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -57,10 +57,6 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); ~HesaiRosWrapper(); - /// @brief Callback for PandarScan subscriber - /// @param scan_msg Received PandarScan - void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg); - /// @brief Get current status of this driver /// @return Current status Status GetStatus(); @@ -133,7 +129,7 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase const drivers::SensorConfigurationBase & sensor_configuration) override; /// @brief Callback for receiving PandarScan /// @param scan_buffer Received PandarScan - void ReceiveScanDataCallback(std::unique_ptr scan_buffer); + void ReceiveCloudPacketCallback(const std::vector & scan_buffer); /// @brief rclcpp parameter callback /// @param parameters Received parameters diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 4fc2a12df..b809bccf8 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -69,7 +69,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) #endif hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1)); + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); pandar_scan_pub_ = this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); @@ -117,9 +117,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - pandar_scan_sub_ = create_subscription( - "pandar_packets", qos, - std::bind(&HesaiRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); + nebula_points_pub_ = this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); aw_points_base_pub_ = @@ -198,16 +196,22 @@ cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); } } -void HesaiRosWrapper::ReceiveScanMsgCallback( - const pandar_msgs::msg::PandarScan::SharedPtr scan_msg) +void HesaiRosWrapper::ReceiveCloudPacketCallback( + const std::vector & packet) { + // Driver is not initialized yet + if (!driver_ptr_) { + return; + } + auto t_start = std::chrono::high_resolution_clock::now(); std::tuple pointcloud_ts = - driver_ptr_->ConvertScanToPointcloud(scan_msg); + driver_ptr_->ParseCloudPacket(packet); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); if (pointcloud == nullptr) { - RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); + // todo + // RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; }; if ( @@ -845,15 +849,6 @@ Status HesaiRosWrapper::InitializeHwMonitor( // todo: don't think this is neede return Status::OK; } -void HesaiRosWrapper::ReceiveScanDataCallback( - std::unique_ptr scan_buffer) -{ - // Publish - scan_buffer->header.frame_id = sensor_cfg_ptr_->frame_id; - scan_buffer->header.stamp = scan_buffer->packets.front().stamp; - pandar_scan_pub_->publish(std::move(scan_buffer)); -} - rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( const std::vector & p) {