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 f38bfc842..cef5a72e6 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_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 97cc4980c..e6dea54e4 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -85,7 +85,7 @@ int Vlp16Decoder::points_per_packet() void Vlp16Decoder::reset_pointcloud(double time_stamp) { - scan_pc_->points.clear(); + scan_pc_->clear(); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 83ebe9472..d97b6edab 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -83,8 +83,7 @@ int Vlp32Decoder::points_per_packet() void Vlp32Decoder::reset_pointcloud(double time_stamp) { - // scan_pc_.reset(new NebulaPointCloud); - scan_pc_->points.clear(); + scan_pc_->clear(); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index 0e59b5fd3..cb2555f99 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -86,8 +86,7 @@ int Vls128Decoder::points_per_packet() void Vls128Decoder::reset_pointcloud(double time_stamp) { - // scan_pc_.reset(new NebulaPointCloud); - scan_pc_->points.clear(); + scan_pc_->clear(); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } 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 9c653dedd..2c7e08d00 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/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index 7f5fa8f0c..c42ca2ee3 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -4,6 +4,7 @@ #include "nebula_ros/common/parameter_descriptors.hpp" +#include #include namespace nebula::ros @@ -13,7 +14,8 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( const std::shared_ptr & hw_interface, std::shared_ptr & config) : logger_(parent_node->get_logger().get_child("HwMonitor")), - diagnostics_updater_(parent_node), + diagnostics_updater_( + (parent_node->declare_parameter("diagnostic_updater.use_fqn", true), parent_node)), status_(Status::OK), hw_interface_(hw_interface), parent_node_(parent_node) diff --git a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp index 83e024d3d..85d34bac9 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) { diff --git a/nebula_tests/data/hesai/ot128/1730271167765338806/1730271167765338806_0.db3 b/nebula_tests/data/hesai/ot128/1730271167765338806/1730271167765338806_0.db3 new file mode 100644 index 000000000..b59012187 Binary files /dev/null and b/nebula_tests/data/hesai/ot128/1730271167765338806/1730271167765338806_0.db3 differ diff --git a/nebula_tests/data/hesai/ot128/1730271167765338806/metadata.yaml b/nebula_tests/data/hesai/ot128/1730271167765338806/metadata.yaml new file mode 100644 index 000000000..ee72ff517 --- /dev/null +++ b/nebula_tests/data/hesai/ot128/1730271167765338806/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: "" + duration: + nanoseconds: 98498380 + starting_time: + nanoseconds_since_epoch: 1730271167765338806 + message_count: 2 + topics_with_message_count: + - topic_metadata: + name: /pandar_packets + type: pandar_msgs/msg/PandarScan + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 5\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 2 + compression_format: "" + compression_mode: "" + relative_file_paths: + - 1730271167765338806_0.db3 + files: + - path: 1730271167765338806_0.db3 + starting_time: + nanoseconds_since_epoch: 1730271167765338806 + duration: + nanoseconds: 98498380 + message_count: 2 diff --git a/nebula_tests/data/hesai/ot128/1730271167863837186.pcd b/nebula_tests/data/hesai/ot128/1730271167863837186.pcd new file mode 100644 index 000000000..6c32cca99 Binary files /dev/null and b/nebula_tests/data/hesai/ot128/1730271167863837186.pcd differ diff --git a/nebula_tests/data/hesai/qt128/1730273789074637152/1730273789074637152_0.db3 b/nebula_tests/data/hesai/qt128/1730273789074637152/1730273789074637152_0.db3 new file mode 100644 index 000000000..1985bf351 Binary files /dev/null and b/nebula_tests/data/hesai/qt128/1730273789074637152/1730273789074637152_0.db3 differ diff --git a/nebula_tests/data/hesai/qt128/1730273789074637152/metadata.yaml b/nebula_tests/data/hesai/qt128/1730273789074637152/metadata.yaml new file mode 100644 index 000000000..d3f215806 --- /dev/null +++ b/nebula_tests/data/hesai/qt128/1730273789074637152/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: "" + duration: + nanoseconds: 100122457 + starting_time: + nanoseconds_since_epoch: 1730273789074637152 + message_count: 2 + topics_with_message_count: + - topic_metadata: + name: /pandar_packets + type: pandar_msgs/msg/PandarScan + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 5\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 2 + compression_format: "" + compression_mode: "" + relative_file_paths: + - 1730273789074637152_0.db3 + files: + - path: 1730273789074637152_0.db3 + starting_time: + nanoseconds_since_epoch: 1730273789074637152 + duration: + nanoseconds: 100122457 + message_count: 2 diff --git a/nebula_tests/data/hesai/qt128/1730273789174759609.pcd b/nebula_tests/data/hesai/qt128/1730273789174759609.pcd new file mode 100644 index 000000000..0a19b6a46 Binary files /dev/null and b/nebula_tests/data/hesai/qt128/1730273789174759609.pcd differ diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index ad72babc7..1b98f91fe 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -24,7 +24,7 @@ namespace nebula::test { -const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { +const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[8] = { {"Pandar40P", "Dual", "Pandar40P.csv", "40p/1673400149412331409", "hesai", 0, 0.0, 0., 360., 0.3f, 200.f}, {"Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", "hesai", 0, 0.0, 0., 360., 0.3f, @@ -36,7 +36,11 @@ const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { {"PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", "hesai", 0, 0.0, 0., 360., 0.05f, 120.f}, {"PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", "hesai", 0, 0.0, - 0., 360., 0.5f, 300.f}}; + 0., 360., 0.5f, 300.f}, + {"PandarQT128", "LastStrongest", "PandarQT128.csv", "qt128/1730273789074637152", "hesai", 0, 0.0, + 0., 360., 0.3f, 300.f}, + {"Pandar128E4X", "LastStrongest", "Pandar128E4X.csv", "ot128/1730271167765338806", "hesai", 0, + 0.0, 0., 360., 0.3f, 300.f}}; // Compares geometrical output of decoder against pre-recorded reference pointcloud. TEST_P(DecoderTest, TestPcd)