From b1d7a7c8490c20883d30d8ec4606609e4ba627c4 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Wed, 30 Oct 2024 21:01:29 +0900 Subject: [PATCH] fix(robosense): record/replay DIFOP packets as well and do not crash on replay start Signed-off-by: Max SCHMELLER --- .../robosense_driver.hpp | 2 +- .../robosense/robosense_ros_wrapper.hpp | 8 +++- .../src/robosense/robosense_ros_wrapper.cpp | 42 +++++++++++++++---- 3 files changed, 42 insertions(+), 10 deletions(-) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp index f38bfc84..cef5a72e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp @@ -34,7 +34,7 @@ class RobosenseDriver : NebulaDriverBase { private: /// @brief Current driver status - Status driver_status_; + Status driver_status_{Status::NOT_INITIALIZED}; /// @brief Decoder according to the model std::shared_ptr scan_decoder_; diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp index 9c653ded..2c7e08d0 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include @@ -81,14 +82,17 @@ class RobosenseRosWrapper final : public rclcpp::Node nebula::Status wrapper_status_; - std::shared_ptr sensor_cfg_ptr_{}; + std::shared_ptr sensor_cfg_ptr_; /// @brief Stores received packets that have not been processed yet by the decoder thread MtQueue> packet_queue_; /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; - rclcpp::Subscription::SharedPtr packets_sub_{}; + rclcpp::Publisher::SharedPtr info_packets_pub_; + + rclcpp::Subscription::SharedPtr packets_sub_; + rclcpp::Subscription::SharedPtr info_packets_sub_; bool launch_hw_; diff --git a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp index 83e024d3..85d34bac 100644 --- a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp @@ -4,6 +4,13 @@ #include "nebula_ros/common/parameter_descriptors.hpp" +#include + +#include + +#include +#include + #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" namespace nebula::ros @@ -12,10 +19,7 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("robosense_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), sensor_cfg_ptr_(nullptr), - packet_queue_(3000), - hw_interface_wrapper_(), - hw_monitor_wrapper_(), - decoder_wrapper_() + packet_queue_(3000) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -28,23 +32,28 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); + info_driver_.emplace(sensor_cfg_ptr_); + launch_hw_ = declare_parameter("launch_hw", param_read_only()); if (launch_hw_) { hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); hw_monitor_wrapper_.emplace(this, sensor_cfg_ptr_); - info_driver_.emplace(sensor_cfg_ptr_); } RCLCPP_DEBUG(get_logger(), "Starting stream"); decoder_thread_ = std::thread([this]() { while (true) { + if (!decoder_wrapper_) continue; decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); } }); if (launch_hw_) { + info_packets_pub_ = + create_publisher("robosense_info_packets", 10); + hw_interface_wrapper_->hw_interface()->register_scan_callback( std::bind(&RobosenseRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); hw_interface_wrapper_->hw_interface()->register_info_callback( @@ -54,9 +63,16 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) packets_sub_ = create_subscription( "robosense_packets", rclcpp::SensorDataQoS(), std::bind(&RobosenseRosWrapper::receive_scan_message_callback, this, std::placeholders::_1)); + info_packets_sub_ = create_subscription( + "robosense_info_packets", 10, [this](const robosense_msgs::msg::RobosenseInfoPacket & msg) { + std::vector raw_packet; + std::copy(msg.packet.data.cbegin(), msg.packet.data.cend(), std::back_inserter(raw_packet)); + receive_info_packet_callback(raw_packet); + }); RCLCPP_INFO_STREAM( - get_logger(), - "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + get_logger(), "Hardware connection disabled, listening for packets on " + << packets_sub_->get_topic_name() << " and " + << info_packets_sub_->get_topic_name()); } // Register parameter callback after all params have been declared. Otherwise it would be called @@ -153,6 +169,18 @@ void RobosenseRosWrapper::receive_info_packet_callback(std::vector & pa "Wrapper already receiving packets despite not being fully initialized yet."); } + if (info_packets_pub_) { + robosense_msgs::msg::RobosenseInfoPacket info_packet{}; + if (packet.size() != info_packet.packet.data.size()) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, "Could not publish info packet: size unsupported"); + } else { + std::copy(packet.cbegin(), packet.cend(), info_packet.packet.data.begin()); + info_packet.packet.stamp = now(); + info_packets_pub_->publish(info_packet); + } + } + auto status = info_driver_->decode_info_packet(packet); if (status != nebula::Status::OK) {