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 6f738c0b1..303bd0c3c 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 @@ -17,6 +17,8 @@ // Have to define macros to silence warnings about deprecated headers being used by // boost/property_tree/ in some versions of boost. // See: https://github.com/boostorg/property_tree/issues/51 +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" + #include #include @@ -32,7 +34,6 @@ #include #include -#include #include #include #include @@ -128,12 +129,11 @@ class HesaiHwInterface using ptc_cmd_result_t = nebula::util::expected, ptc_error_t>; std::shared_ptr logger_; - std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; + std::optional udp_socket_; std::shared_ptr m_owned_ctx_; - std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_; std::shared_ptr sensor_configuration_; - std::function & buffer)> + std::function & buffer)> cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ std::mutex mtx_inflight_tcp_request_; @@ -198,7 +198,7 @@ class HesaiHwInterface /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void receive_sensor_packet_callback(std::vector & buffer); + void receive_sensor_packet_callback(const std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status Status sensor_interface_start(); @@ -221,7 +221,7 @@ class HesaiHwInterface /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status register_scan_callback(std::function &)> scan_callback); + Status register_scan_callback(std::function &)> scan_callback); /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION /// @return Resulting status std::string get_lidar_calibration_string(); 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 f25344bf2..8d68c2111 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 @@ -7,6 +7,7 @@ #include "nebula_common/loggers/logger.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/udp.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" #include @@ -15,6 +16,7 @@ #include #include +#include #include #include #include @@ -39,9 +41,7 @@ using nlohmann::json; HesaiHwInterface::HesaiHwInterface(const std::shared_ptr & logger) : logger_(logger), - cloud_io_context_{new ::drivers::common::IoContext(1)}, m_owned_ctx_{new boost::asio::io_context(1)}, - cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, tcp_driver_{new ::drivers::tcp_driver::TcpDriver(m_owned_ctx_)}, target_model_no_(nebula_model_to_hesai_model_no(SensorModel::UNKNOWN)) { @@ -119,7 +119,7 @@ HesaiHwInterface::ptc_cmd_result_t HesaiHwInterface::send_receive( // Header had payload length 0 (thus, header callback processed request successfully already), // but we still received a payload: invalid state - if (*response_complete == true) { + if (*response_complete) { error_code->error_flags |= g_tcp_error_unexpected_payload; return; } @@ -165,66 +165,51 @@ Status HesaiHwInterface::set_sensor_configuration( Status HesaiHwInterface::sensor_interface_start() { + auto builder = connections::UdpSocket::Builder( + sensor_configuration_->host_ip, sensor_configuration_->data_port); + if (!sensor_configuration_->multicast_ip.empty()) { + builder.join_multicast_group(sensor_configuration_->multicast_ip); + } + + builder.set_mtu(g_mtu_size); + try { - logger_->info("Starting UDP receiver"); - if (sensor_configuration_->multicast_ip.empty()) { - cloud_udp_driver_->init_receiver( - sensor_configuration_->host_ip, sensor_configuration_->data_port); - } else { - cloud_udp_driver_->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port); - cloud_udp_driver_->receiver()->setMulticast(true); - } -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - logger_->error("init ok"); -#endif - cloud_udp_driver_->receiver()->open(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - logger_->error("open ok"); -#endif + builder.set_socket_buffer_size(g_udp_socket_buffer_size); + } catch (const connections::SocketError & e) { + throw std::runtime_error( + "Could not set socket receive buffer size to " + std::to_string(g_udp_socket_buffer_size) + + ". Try increasing net.core.rmem_max."); + } - bool success = cloud_udp_driver_->receiver()->setKernelBufferSize(g_udp_socket_buffer_size); - if (!success) { - logger_->error( - "Could not set receive buffer size. Try increasing net.core.rmem_max to " + - std::to_string(g_udp_socket_buffer_size) + " B."); - return Status::ERROR_1; - } + udp_socket_.emplace(std::move(builder).bind()); - cloud_udp_driver_->receiver()->bind(); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - logger_->error("bind ok"); -#endif + udp_socket_->subscribe([&]( + const std::vector & packet, + const connections::UdpSocket::RxMetadata & /* metadata */) { + receive_sensor_packet_callback(packet); + }); - cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&HesaiHwInterface::receive_sensor_packet_callback, this, std::placeholders::_1)); -#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - logger_->error("async receive set"); -#endif - } catch (const std::exception & ex) { - Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << " for " << sensor_configuration_->sensor_ip << ":" - << sensor_configuration_->data_port << " - " << ex.what() << std::endl; - return status; - } return Status::OK; } Status HesaiHwInterface::register_scan_callback( - std::function &)> scan_callback) + std::function &)> scan_callback) { cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void HesaiHwInterface::receive_sensor_packet_callback(std::vector & buffer) +void HesaiHwInterface::receive_sensor_packet_callback(const std::vector & buffer) { cloud_packet_callback_(buffer); } + Status HesaiHwInterface::sensor_interface_stop() { - return Status::ERROR_1; + if (udp_socket_) { + udp_socket_->unsubscribe(); + } + return Status::OK; } Status HesaiHwInterface::get_sensor_configuration( 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 8bf0100ff..e5d62fcb0 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -50,7 +50,11 @@ class HesaiRosWrapper final : public rclcpp::Node public: explicit HesaiRosWrapper(const rclcpp::NodeOptions & options); - ~HesaiRosWrapper() noexcept override = default; + ~HesaiRosWrapper() noexcept override + { + if (!hw_interface_wrapper_) return; + hw_interface_wrapper_->hw_interface()->sensor_interface_stop(); + }; /// @brief Get current status of this driver /// @return Current status @@ -61,7 +65,7 @@ class HesaiRosWrapper final : public rclcpp::Node Status stream_start(); private: - void receive_cloud_packet_callback(std::vector & packet); + void receive_cloud_packet_callback(const std::vector & packet); void receive_scan_message_callback(std::unique_ptr scan_msg); diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 387d6c4c7..c1af93bac 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -222,6 +223,13 @@ Status HesaiRosWrapper::validate_and_set_config( if (new_config->frame_id.empty()) { return Status::SENSOR_CONFIG_ERROR; } + if (new_config->host_ip == "255.255.255.255") { + RCLCPP_ERROR( + get_logger(), + "Due to potential network performance issues when using IP broadcast for sensor data, Nebula " + "disallows use of the broadcast IP. Please specify the concrete host IP instead."); + return Status::SENSOR_CONFIG_ERROR; + } if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) { RCLCPP_ERROR( get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); @@ -411,7 +419,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( return rcl_interfaces::build().successful(true).reason(""); } -void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packet) +void HesaiRosWrapper::receive_cloud_packet_callback(const std::vector & packet) { if (!decoder_wrapper_ || decoder_wrapper_->status() != Status::OK) { return; @@ -424,7 +432,7 @@ void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packe 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.swap(packet); + msg_ptr->data = packet; decoder_wrapper_->process_cloud_packet(std::move(msg_ptr)); }