diff --git a/build_depends.repos b/build_depends.repos index e28379f1a..b7ac379a9 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -2,5 +2,5 @@ repositories: # TCP version of transport drivers transport_drivers: type: git - url: https://github.com/MapIV/transport_drivers - version: boost + url: https://github.com/InnovizTechnologies/transport_drivers + version: configurable_udp_recv_buffer_size diff --git a/nebula_common/include/nebula_common/innoviz/innoviz_common.hpp b/nebula_common/include/nebula_common/innoviz/innoviz_common.hpp new file mode 100644 index 000000000..557490eeb --- /dev/null +++ b/nebula_common/include/nebula_common/innoviz/innoviz_common.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" + +namespace nebula +{ +namespace drivers +{ + struct InnovizSensorConfiguration : SensorConfigurationBase + { + uint8_t min_confidence = 21; + bool filter_artifacts = true; + }; +} +} \ No newline at end of file diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index d8648ee11..b4a61292f 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -323,6 +323,8 @@ enum class SensorModel { VELODYNE_VLP32MR, VELODYNE_HDL32, VELODYNE_VLP16, + INNOVIZ_TWO_RAVEN, + INNOVIZ_TWO_CONDOR }; /// @brief not used? @@ -401,6 +403,12 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::VELODYNE_VLP16: os << "VLP16"; break; + case SensorModel::INNOVIZ_TWO_RAVEN: + os << "InnovizTwoRaven"; + break; + case SensorModel::INNOVIZ_TWO_CONDOR: + os << "InnovizTwoCondor"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -468,6 +476,8 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "VLP32MR") return SensorModel::VELODYNE_VLP32MR; if (sensor_model == "HDL32") return SensorModel::VELODYNE_HDL32; if (sensor_model == "VLP16") return SensorModel::VELODYNE_VLP16; + if (sensor_model == "InnovizTwoRaven") return SensorModel::INNOVIZ_TWO_RAVEN; + if (sensor_model == "InnovizTwoCondor") return SensorModel::INNOVIZ_TWO_CONDOR; return SensorModel::UNKNOWN; } diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index cb0f3bb2f..9a5ff6428 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -48,6 +48,13 @@ ament_auto_add_library(nebula_decoders_velodyne SHARED src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp ) +# Innoviz +ament_auto_add_library(nebula_decoders_innoviz SHARED + src/nebula_decoders_innoviz/innoviz_driver.cpp + src/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.cpp + src/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.cpp + src/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.cpp) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_decoders/calibration/innoviz/InnovizTwoCondor.csv b/nebula_decoders/calibration/innoviz/InnovizTwoCondor.csv new file mode 100644 index 000000000..e69de29bb diff --git a/nebula_decoders/calibration/innoviz/InnovizTwoRaven.csv b/nebula_decoders/calibration/innoviz/InnovizTwoRaven.csv new file mode 100644 index 000000000..e69de29bb diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.hpp new file mode 100644 index 000000000..945bebe7a --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.hpp @@ -0,0 +1,187 @@ +#pragma once + +#include + +#include "nebula_common/point_types.hpp" +#include "nebula_common/innoviz/innoviz_common.hpp" + +#include "innoviz_msgs/msg/innoviz_packet.hpp" + +namespace nebula +{ +namespace drivers +{ + +#pragma pack(push, 1) + +struct innoviz_network_al_st { + uint32_t marker; // 0xFACE_CAFE + uint32_t pc_idx; // iPCL payload Index for Reassembly + uint16_t innoviz_network_al_version; // Default set to 0 + uint16_t protocol_type; // The type of protocol adaptation layer - iPCL protocol type is set to 0x1. + uint32_t data_fregmantaton_type; // Default set to 0 + uint32_t sequence_number; // Incremented by 1 for every packet sent + uint32_t reserved; // Should be set to zero + uint32_t start_idx; //First object index for each fragment. + uint32_t end_idx; // End object index. + uint32_t total_objects; // this is the total objects; Currently Object represented by 1 byte +}; + +struct EventDQ_et{ + uint8_t value; +}; + +struct ExEventDQ_et{ + uint16_t value; +}; + +struct PowerMode_et{ + uint8_t value; +}; + +struct Radian_et{ + float value; +}; + +struct Meter_et{ + float value; +}; + +struct Centimeter_st{ + uint16_t value; +}; + +struct InterfaceVersionId_st{ + uint8_t major; + uint8_t minor; + uint16_t patch; +}; + +struct SensorPose_st{ + uint16_t invalid_flags; + Meter_et x; + Meter_et x_std_dev; + Meter_et y; + Meter_et y_std_dev; + Meter_et z; + Meter_et z_std_dev; + Radian_et roll; + Radian_et roll_std_dev; + Radian_et pitch; + Radian_et pitch_std_dev; + Radian_et yaw; + Radian_et yaw_std_dev; +}; + +struct StndTimestamp_st{ + uint64_t seconds; + uint32_t nano_seconds; + uint32_t lidar_internal_timer; + uint8_t sync_status; // Sync status +}; + +struct Classification_et{ + uint16_t value; +}; + +struct Sensor_metadata_et{ + uint32_t value[8]; +}; + +struct Pcp_st{ + uint8_t ppv; // Positive predicted value + uint8_t height; + Classification_et classification; // +}; + +union Extension_ut{ + Pcp_st pcp; // PCP extension +}; + +// Structure definitions +struct Position_st{ + Centimeter_st distance; + int16_t angle_azimuth; // axis Definition based on ISO23150 + int16_t angle_elevation; // axis Definition based on ISO23150 +}; + +union Signature_ut{ + uint32_t crc_32_type1; + uint32_t crc_32_type2; + uint32_t crc_32_type3; + uint32_t autentication_128[4]; + uint32_t autentication_256[8]; +}; + +// The Pixel Structure - 10 bytes Size - Core , PCP extension - 4 bytes +struct LidarDetectionEntity_st{ + Centimeter_st distance; + uint8_t confidence; + uint8_t reflectivity; + int16_t angle_azimuth; // axis Definition based on ISO23150 (MUST - Need to run Teta/Phi to Cartesian trasformation) + int16_t angle_elevation; // axis Definition based on ISO23150 (MUST - Need to run Teta/Phi to Cartesian trasformation) + uint16_t Invalid_detection_classification; // Invalid classes - Artifacts classes as example (DEFAULR - 0x0 - No artifacts) + /*---------------------------------------------------------------------------------*/ + // Extension_ut detection_entity_extension; // Extension – should be align to 2 bytes + /*---------------------------------------------------------------------------------*/ +}; + +// Point Cloud Header Structure +struct LidarSensorDetectionsHeader_st{ + InterfaceVersionId_st version_id; // Interface Version ID (DEFAULT 0.1.0) + uint32_t frame_id; // Frame ID from LiDAR raw data (VALID - Taken from INVZ) + Sensor_metadata_et metadata; // Sensor Metadata For Sensor periodic information (DEFAULT - first 4 bytes - 0x01 (Sensor ID - Raven Center), byte 5-8 Lidar FW App Version, other 24 bytes set to 0x0) + uint32_t number_of_detections; // Number of detection points (<= Max Detection points) (VALID from the INVZ - mumber of pixels in the frame) + uint8_t pixel_struct_type; // Pixel extension type (DEFAULT - - 0x0 - Basic structure , No extensions) + EventDQ_et event_data_qualifier; // Qualifiers ENUM (DEFAULT - 0x0) + ExEventDQ_et extended_qualifier; // Extended Qualifiers ENUM - DEFAULT (0x0) + PowerMode_et lidar_power_mode; // Power mode ENUM (NPM - 0x0 - No other modes for now) + StndTimestamp_st timestamp; // Frame Timestamp (MUST - Wall clock and sync status / Internal timer if out of sync) + SensorPose_st sensor_pose; // Sensor position – Calibration information (DEFAUT - Instalation initial coordinates - Hardcoded value) +} ; + +// Point cloud interface structure - iPCL (r1.0) +struct LidarDetectionInterface_st{ + uint32_t detection_interface_type; // To be used for Interface formats (DEFAULT: 0x1 - type 1 - No signature) + uint32_t length; // Total length in Bytes + LidarSensorDetectionsHeader_st lidar_sensor_detections_header; + std::vector detections; // +}; + + +#pragma pack(pop) + +/// @brief Base class for Innoviz LiDAR decoder +class InnovizScanDecoder +{ +public: + InnovizScanDecoder(uint32_t numOfPoints, const std::shared_ptr& sensorConfiguration); + + /// @brief Unpack UDP data + /// @param buf pointer to UDP payload + void parsePacket(innoviz_msgs::msg::InnovizPacket& packet); + + /// @brief Checks if decoder holds a complete frame. + /// @return True if the decoder holds a complete frame. False otherwise. + bool isFrameComplete(); + + /// @brief Gets a completed pointcloud in Nebula format. + /// @return Nebula format pointcloud. + drivers::NebulaPointCloudPtr getPointcloud(); + + /// @brief Resets the pointcloud. Should be called between frames. + void resetPointcloud(); + +private: + NebulaPointCloudPtr nebula_scan_pc_; + LidarDetectionInterface_st innoviz_scan_pc_; + + size_t written_size_ = 0; + size_t scan_size_ = 0; + uint32_t last_sequence_number_ = 0; + std::shared_ptr sensor_configuration_; + +}; + +} +} diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.hpp new file mode 100644 index 000000000..9a6a90b15 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include "innoviz_scan_decoder.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace itwo_condor +{ + +constexpr uint32_t MAX_POINTS = 16*32*600; + +/// @brief Class for Innoviz Two Condor decoder +class InnovizTwoCondor : public InnovizScanDecoder +{ +public: + InnovizTwoCondor(const std::shared_ptr& sensorConfiguration); +}; + +} +} +} \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.hpp new file mode 100644 index 000000000..c755b62f9 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include "innoviz_scan_decoder.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace itwo_raven +{ + +constexpr uint32_t MAX_POINTS = 30*32*600; + +/// @brief Class for Innoviz Two Raven decoder +class InnovizTwoRaven : public InnovizScanDecoder +{ +public: + InnovizTwoRaven(const std::shared_ptr& sensorConfiguration); +}; + +} +} +} \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/innoviz_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/innoviz_driver.hpp new file mode 100644 index 000000000..5bccaac41 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_innoviz/innoviz_driver.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "innoviz_msgs/msg/innoviz_scan.hpp" +#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" +#include "nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.hpp" +#include "nebula_common/innoviz/innoviz_common.hpp" + +namespace nebula +{ +namespace drivers +{ + +/// @brief Innoviz Driver +class InnovizDriver : public NebulaDriverBase +{ +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this driver + InnovizDriver(const std::shared_ptr& sensorConfiguration); + + /// @brief Convert InnovizScan message to point cloud + /// @param innoviz_scan InnovizScan message to convert + /// @return Point cloud + drivers::NebulaPointCloudPtr ConvertScanToPointcloud(const std::shared_ptr & innoviz_scan); + + /// @brief Setting CalibrationConfiguration (not used) + /// @param calibration_configuration + /// @return Resulting status + Status SetCalibrationConfiguration(const CalibrationConfigurationBase & calibration_configuration); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + +private: + std::shared_ptr scan_decoder_; + Status driver_status_; +}; + +} +} + diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index f25fc698a..61a068c27 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -19,6 +19,7 @@ pcl_conversions sensor_msgs velodyne_msgs + innoviz_msgs yaml-cpp ament_cmake_gtest diff --git a/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.cpp b/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.cpp new file mode 100644 index 000000000..933e97587 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.cpp @@ -0,0 +1,188 @@ + +#include "nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_scan_decoder.hpp" +#include +#include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#define ANGLE_ELEVATION_AZIMUTH_FACTOR (1.f/32768) +#define INNOVIZ_IPCL_MARKER (0xfacecafe) + + +namespace nebula +{ +namespace drivers +{ + +InnovizScanDecoder::InnovizScanDecoder(uint32_t numOfPoints, const std::shared_ptr& sensorConfiguration) +{ + sensor_configuration_ = sensorConfiguration; + innoviz_scan_pc_.detections.resize(numOfPoints); + nebula_scan_pc_.reset(new NebulaPointCloud); + nebula_scan_pc_->reserve(innoviz_scan_pc_.detections.size()); +} + + +void InnovizScanDecoder::resetPointcloud() +{ + written_size_ = 0; + scan_size_ = 0; + + innoviz_scan_pc_.detection_interface_type = 0; + innoviz_scan_pc_.length = 0; + memset(&innoviz_scan_pc_.lidar_sensor_detections_header, 0, sizeof(LidarSensorDetectionsHeader_st)); + memset(innoviz_scan_pc_.detections.data(), 0, innoviz_scan_pc_.detections.size() * sizeof(LidarDetectionEntity_st)); +} + +void InnovizScanDecoder::parsePacket(innoviz_msgs::msg::InnovizPacket& packet) +{ + uint32_t marker = packet.header.marker; + uint32_t sequence_number = packet.header.sequence_number; + uint32_t start_id = packet.header.start_id; + uint32_t end_id = packet.header.end_idx; + uint32_t total_objects = packet.header.total_objects; + uint32_t length = end_id - start_id; + uint32_t offset = 0; + + constexpr uint32_t INVZ_PREFIX_SIZE = sizeof(innoviz_scan_pc_.length) + + sizeof(innoviz_scan_pc_.detection_interface_type) + + sizeof(innoviz_scan_pc_.lidar_sensor_detections_header); + + //Check packet has a valid Innoviz marker + if(marker != INNOVIZ_IPCL_MARKER) + { + std::cerr << "InnovizScanDecoder: Invalid marker" << std::endl; + std::cerr << "Marker = " << marker << std::endl; + return; + } + + //Check for missing packet sequence number + if(start_id != 0 &&last_sequence_number_ != 0 && + ((last_sequence_number_ == UINT32_MAX && sequence_number != 0) || + last_sequence_number_ + 1 != sequence_number)) + { + std::cerr << "InnovizScanDecoder: Missing packet" << std::endl; + } + + //Ensure validity of scan_size + if(scan_size_ != 0 && scan_size_ != total_objects) + { + std::cerr << "InnovizScanDecoder: Scan has inconsistent size" << std::endl; + } + + if(start_id >= end_id) + { + std::cerr << "InnovizScanDecoder: Invalid start/end id" << std::endl; + return; + } + + //Check for overflow + uint32_t totalStructSize = INVZ_PREFIX_SIZE + innoviz_scan_pc_.detections.size() * sizeof(LidarDetectionEntity_st); + if((start_id + length) > totalStructSize) + { + std::cerr << "InnovizScanDecoder: Scan overflow" << std::endl; + auto sum = start_id + length; + std::cerr << "Writing to " + sum << " in struct of size " << totalStructSize << std::endl; + return; + } + + //Write to prefix first + if(start_id < INVZ_PREFIX_SIZE) + { + uint32_t prefixLength = std::min(INVZ_PREFIX_SIZE - start_id, length); + uint8_t* destination = ((uint8_t *)(&innoviz_scan_pc_)) + start_id; + memcpy(destination, packet.data.data(), prefixLength); + start_id += prefixLength; + length -= prefixLength; + written_size_ += prefixLength; + offset += prefixLength; + } + + //Logically this can only happen if length < INVZ_PREFIX_SIZE. + // If this happens, we have no more data to write from this packet. + if(start_id < INVZ_PREFIX_SIZE) + { + return; + } + + //Discount INVZ_PREFIX_SIZE when writing to detections vector + start_id -= INVZ_PREFIX_SIZE; + uint8_t* destination = (uint8_t *)(innoviz_scan_pc_.detections.data()) + start_id; + + + memcpy(destination, packet.data.data() + offset, length); + + scan_size_ = total_objects; + written_size_ += length; + last_sequence_number_ = sequence_number; + +} + +bool InnovizScanDecoder::isFrameComplete() +{ + return scan_size_ != 0 && written_size_ == scan_size_; +} + +drivers::NebulaPointCloudPtr InnovizScanDecoder::getPointcloud() +{ + if(!isFrameComplete()) + { + std::cerr << "Missing packets in frame. Returning empty frame" << std::endl; + nebula_scan_pc_->points.clear(); + return nebula_scan_pc_; + } + + nebula_scan_pc_->points.clear(); + nebula_scan_pc_->points.reserve(innoviz_scan_pc_.detections.size()); + //Take UTC timestamp from synced LiDAR and convert to micros + nebula_scan_pc_->header.stamp = (innoviz_scan_pc_.lidar_sensor_detections_header.timestamp.seconds * 1e6 + + innoviz_scan_pc_.lidar_sensor_detections_header.timestamp.nano_seconds) * 0.001; + + for(uint32_t pixelID = 0; pixelID < innoviz_scan_pc_.detections.size(); pixelID++) + { + LidarDetectionEntity_st invzPoint = innoviz_scan_pc_.detections[pixelID]; + + bool isValid = invzPoint.distance.value > 0; + + // Filter pixels that are below the configured confidence threshold + if(invzPoint.confidence < sensor_configuration_->min_confidence) + { + isValid = false; + } + + // Filter artifact pixels based on the configuration + if(sensor_configuration_->filter_artifacts && invzPoint.Invalid_detection_classification) + { + isValid = false; + } + + if(isValid) + { + drivers::NebulaPoint nebulaPoint{}; + nebulaPoint.distance = invzPoint.distance.value * 0.01f; // uint16 cm to float m + //Convert digital azimuth/elevation values to radians + nebulaPoint.azimuth = static_cast(invzPoint.angle_azimuth)* M_PI * (ANGLE_ELEVATION_AZIMUTH_FACTOR); + nebulaPoint.elevation = static_cast(invzPoint.angle_elevation)* M_PI * (ANGLE_ELEVATION_AZIMUTH_FACTOR); + + nebulaPoint.x = nebulaPoint.distance * cos(nebulaPoint.azimuth) * cos(nebulaPoint.elevation); + nebulaPoint.y = nebulaPoint.distance * sin(nebulaPoint.azimuth) * cos(nebulaPoint.elevation); + nebulaPoint.z = nebulaPoint.distance * sin(nebulaPoint.elevation); + + nebulaPoint.intensity = invzPoint.reflectivity; + nebulaPoint.return_type = static_cast(drivers::ReturnType::STRONGEST); + + //TODO: Inhabit channel and time_stamp + + nebula_scan_pc_->points.emplace_back(nebulaPoint); + } + } + + return nebula_scan_pc_; +} + +} +} diff --git a/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.cpp b/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.cpp new file mode 100644 index 000000000..00842c985 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.cpp @@ -0,0 +1,15 @@ + +#include "nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace itwo_condor +{ + InnovizTwoCondor::InnovizTwoCondor(const std::shared_ptr& sensorConfiguration) + : InnovizScanDecoder(MAX_POINTS, sensorConfiguration) + {} +} +} +} \ No newline at end of file diff --git a/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.cpp b/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.cpp new file mode 100644 index 000000000..3c1ece17e --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.cpp @@ -0,0 +1,15 @@ + +#include "nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace itwo_raven +{ + InnovizTwoRaven::InnovizTwoRaven(const std::shared_ptr& sensorConfiguration) + : InnovizScanDecoder(MAX_POINTS, sensorConfiguration) + {} +} +} +} \ No newline at end of file diff --git a/nebula_decoders/src/nebula_decoders_innoviz/innoviz_driver.cpp b/nebula_decoders/src/nebula_decoders_innoviz/innoviz_driver.cpp new file mode 100644 index 000000000..7bce9ad7d --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_innoviz/innoviz_driver.cpp @@ -0,0 +1,73 @@ + +#include "nebula_decoders/nebula_decoders_innoviz/innoviz_driver.hpp" +#include "nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_condor_decoder.hpp" +#include "nebula_decoders/nebula_decoders_innoviz/decoders/innoviz_two_raven_decoder.hpp" + + +namespace nebula +{ +namespace drivers +{ + +InnovizDriver::InnovizDriver(const std::shared_ptr& sensorConfiguration) +{ + driver_status_ = nebula::Status::OK; + switch(sensorConfiguration->sensor_model) + { + case SensorModel::INNOVIZ_TWO_CONDOR: + { + scan_decoder_ = std::make_shared(sensorConfiguration); + break; + } + case SensorModel::INNOVIZ_TWO_RAVEN: + { + scan_decoder_ = std::make_shared(sensorConfiguration); + break; + } + default: + driver_status_ = Status::INVALID_SENSOR_MODEL; + + } +} + +Status InnovizDriver::GetStatus() +{ + return driver_status_; +} + + +drivers::NebulaPointCloudPtr InnovizDriver::ConvertScanToPointcloud(const std::shared_ptr & innoviz_scan) +{ + drivers::NebulaPointCloudPtr nebulaPCL; + if(driver_status_ == Status::OK) + { + scan_decoder_->resetPointcloud(); + for(innoviz_msgs::msg::InnovizPacket& packet : innoviz_scan->packets) + { + scan_decoder_->parsePacket(packet); + } + + nebulaPCL = scan_decoder_->getPointcloud(); + + //Check valid timestamp. If the LiDAR is not synced this will be 0 and need to take host timestamp from innoviz_scan + if(nebulaPCL->header.stamp == 0) + { + nebulaPCL->header.stamp = (innoviz_scan->header.stamp.sec * 1e6 + + innoviz_scan->header.stamp.nanosec) * 0.001; + } + } + else + { + nebulaPCL = nullptr; + } + + return nebulaPCL; +} + +Status InnovizDriver::SetCalibrationConfiguration(const CalibrationConfigurationBase & /*calibration_configuration*/) +{ + return Status::NOT_IMPLEMENTED; +} + +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index c1e34ab62..c4c4cf8ba 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -32,6 +32,9 @@ ament_auto_add_library(nebula_hw_interfaces_velodyne SHARED src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp ) +ament_auto_add_library(nebula_hw_interfaces_innoviz SHARED + src/nebula_innoviz_hw_interfaces/innoviz_hw_interface.cpp) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_innoviz/innoviz_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_innoviz/innoviz_hw_interface.hpp new file mode 100644 index 000000000..18b1a7e01 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_innoviz/innoviz_hw_interface.hpp @@ -0,0 +1,79 @@ +#pragma once + + +#include "nebula_common/innoviz/innoviz_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + +#include +#include "boost_udp_driver/udp_driver.hpp" + +#include "innoviz_msgs/msg/innoviz_packet.hpp" +#include "innoviz_msgs/msg/innoviz_scan.hpp" + +namespace nebula +{ +namespace drivers +{ +/// @brief Hardware interface of Innoviz driver +class InnovizHwInterface : NebulaHwInterfaceBase +{ +public: + InnovizHwInterface(); + + /// @brief Virtual function for starting the interface that handles UDP streams + /// @return Resulting status + virtual Status CloudInterfaceStart() override; + + /// @brief Virtual function for stopping the interface that handles UDP streams + /// @return Resulting status + virtual Status CloudInterfaceStop() override; + + /// @brief Virtual function for setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration(std::shared_ptr sensor_configuration) override; + + /// @brief Virtual function for printing sensor configuration + /// @param sensor_configuration SensorConfiguration for the checking + /// @return Resulting status + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) override; + + /// @brief Virtual function for printing calibration configuration + /// @param calibration_configuration CalibrationConfiguration for the checking + /// @return Resulting status + Status GetCalibrationConfiguration(CalibrationConfigurationBase & calibration_configuration) override; + + /// @brief Set callback function to call on completeion of scan read + /// @param scan_callback Callback function to call + void RegisterScanCallback(std::function)> scan_callback); + + /// @brief Setting rclcpp::Logger log messages upstream can be sent upstream + /// @param logger Logger + void SetLogger(std::shared_ptr logger); + +private: + /// @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 ReceiveCloudPacketCallback(const std::vector & buffer); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM if logger is defined. If not prints to stderr + /// @param error Target string + void PrintError(std::string errorMessage); + + +private: + std::shared_ptr parent_node_logger; + std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_; + std::shared_ptr sensor_configuration_; + std::unique_ptr scan_cloud_ptr_; + std::function buffer)> scan_reception_callback_; + + uint32_t processed_bytes_ = 0; + uint32_t last_frame_id_ = 0; + +}; + +} // namespace drivers +} // namespace nebula + diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index aa169d320..34e1f0eb9 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -19,6 +19,7 @@ boost_tcp_driver boost_udp_driver velodyne_msgs + innoviz_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_hw_interfaces/src/nebula_innoviz_hw_interfaces/innoviz_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_innoviz_hw_interfaces/innoviz_hw_interface.cpp new file mode 100644 index 000000000..963297164 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_innoviz_hw_interfaces/innoviz_hw_interface.cpp @@ -0,0 +1,158 @@ + +#include "nebula_hw_interfaces/nebula_hw_interfaces_innoviz/innoviz_hw_interface.hpp" + + +namespace nebula +{ +namespace drivers +{ + InnovizHwInterface::InnovizHwInterface() + : cloud_io_context_{new ::drivers::common::IoContext(1)}, + cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, + scan_cloud_ptr_{std::make_unique()} + {} + + Status InnovizHwInterface::SetSensorConfiguration(std::shared_ptr sensor_configuration) + { + Status status = Status::OK; + sensor_configuration_ = sensor_configuration; + + if(sensor_configuration_->sensor_model != SensorModel::INNOVIZ_TWO_CONDOR && + sensor_configuration_->sensor_model != SensorModel::INNOVIZ_TWO_RAVEN) + { + status = Status::INVALID_SENSOR_MODEL; + } + + if(sensor_configuration_->return_mode != drivers::ReturnMode::SINGLE_FIRST && + sensor_configuration_->return_mode != drivers::ReturnMode::FIRST) + { + status = Status::INVALID_ECHO_MODE; + } + + return status; + } + + Status InnovizHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration){ + sensor_configuration = *sensor_configuration_; + return Status::OK; + } + + Status InnovizHwInterface::GetCalibrationConfiguration(CalibrationConfigurationBase & /*calibration_configuration*/) + { + return Status::ERROR_1; + } + + Status InnovizHwInterface::CloudInterfaceStart() + { + Status status = Status::OK; + try + { + cloud_udp_driver_->init_receiver(sensor_configuration_->host_ip, sensor_configuration_->data_port, UINT16_MAX); + cloud_udp_driver_->receiver()->open(); + cloud_udp_driver_->receiver()->bind(); + cloud_udp_driver_->receiver()->asyncReceive(std::bind(&InnovizHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + } + catch(const std::exception& e) + { + PrintError(e.what()); + status = Status::UDP_CONNECTION_ERROR; + } + + return status; + } + + Status InnovizHwInterface::CloudInterfaceStop() {return Status::ERROR_1;} + + void InnovizHwInterface::ReceiveCloudPacketCallback(const std::vector& buffer) + { + auto now = std::chrono::system_clock::now(); + constexpr uint32_t INVZ_PACKET_HEADER_SIZE = sizeof(innoviz_msgs::msg::InnovizPacketHeader); + uint32_t bufferSize = buffer.size(); + + innoviz_msgs::msg::InnovizPacket innovizPacket; + + if(bufferSize > INVZ_PACKET_HEADER_SIZE) + { + //Populate packet header + memcpy(&innovizPacket.header, buffer.data(), INVZ_PACKET_HEADER_SIZE); + + //Start of new frame before completion of previous one + if(innovizPacket.header.pc_idx != last_frame_id_ && scan_cloud_ptr_->packets.size() > 0) + { + //Send previous scan data (will be a partial frame. Let decoder handle it) + scan_reception_callback_(std::move(scan_cloud_ptr_)); + scan_cloud_ptr_ = std::make_unique(); + scan_cloud_ptr_->packets.clear(); + processed_bytes_ = 0; + } + + uint32_t offset = INVZ_PACKET_HEADER_SIZE; + uint32_t actualDataSize = innovizPacket.header.end_idx - innovizPacket.header.start_id; + + if(buffer.size() <= offset + actualDataSize) + { + //Populate packet data + innovizPacket.data.resize(actualDataSize); + memcpy(innovizPacket.data.data(), buffer.data() + offset, actualDataSize); + + scan_cloud_ptr_->packets.emplace_back(innovizPacket); + + processed_bytes_ += actualDataSize; + + //Start of new scan, update scan timestamp with host time of first packet + if(last_frame_id_ != innovizPacket.header.pc_idx) + { + auto now_nanos = std::chrono::duration_cast(now.time_since_epoch()).count(); + scan_cloud_ptr_->header.stamp.nanosec = now_nanos % ((uint32_t)1e9); + scan_cloud_ptr_->header.stamp.sec = now_nanos / 1e9; + } + last_frame_id_ = innovizPacket.header.pc_idx; + + //Check if completed frame + if(processed_bytes_ == innovizPacket.header.total_objects) + { + //Send completed scan data + scan_reception_callback_(std::move(scan_cloud_ptr_)); + scan_cloud_ptr_ = std::make_unique(); + scan_cloud_ptr_->packets.clear(); + processed_bytes_ = 0; + } + } + else + { + PrintError("Buffer size too small to contain the size described in the packet header"); + } + + + } + else + { + PrintError("Invalid packet size smaller than INVZ_PACKET_HEADER_SIZE"); + } + + } + + void InnovizHwInterface::RegisterScanCallback(std::function)> scan_callback) + { + scan_reception_callback_ = std::move(scan_callback); + } + + void InnovizHwInterface::SetLogger(std::shared_ptr logger) + { + parent_node_logger = logger; + } + + void InnovizHwInterface::PrintError(std::string errorMessage) + { + if(parent_node_logger) + { + RCLCPP_ERROR_STREAM((*parent_node_logger), errorMessage); + } + else + { + std::cout << errorMessage << '\n'; + } + } + +} +} \ No newline at end of file diff --git a/nebula_messages/innoviz_msgs/CMakeLists.txt b/nebula_messages/innoviz_msgs/CMakeLists.txt new file mode 100644 index 000000000..1ca668a86 --- /dev/null +++ b/nebula_messages/innoviz_msgs/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5) +project(innoviz_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/InnovizPacketHeader.msg" + "msg/InnovizPacket.msg" + "msg/InnovizScan.msg" + DEPENDENCIES + std_msgs + ) + +ament_package() diff --git a/nebula_messages/innoviz_msgs/msg/InnovizPacket.msg b/nebula_messages/innoviz_msgs/msg/InnovizPacket.msg new file mode 100644 index 000000000..4e56f1801 --- /dev/null +++ b/nebula_messages/innoviz_msgs/msg/InnovizPacket.msg @@ -0,0 +1,2 @@ +InnovizPacketHeader header +uint8[] data \ No newline at end of file diff --git a/nebula_messages/innoviz_msgs/msg/InnovizPacketHeader.msg b/nebula_messages/innoviz_msgs/msg/InnovizPacketHeader.msg new file mode 100644 index 000000000..13b838c2f --- /dev/null +++ b/nebula_messages/innoviz_msgs/msg/InnovizPacketHeader.msg @@ -0,0 +1,10 @@ +uint32 marker +uint32 pc_idx +uint16 innoviz_network_al_version +uint16 protocol_type +uint32 data_fregmantaton_type +uint32 sequence_number +uint32 reserved +uint32 start_id +uint32 end_idx +uint32 total_objects diff --git a/nebula_messages/innoviz_msgs/msg/InnovizScan.msg b/nebula_messages/innoviz_msgs/msg/InnovizScan.msg new file mode 100644 index 000000000..569dcd2c3 --- /dev/null +++ b/nebula_messages/innoviz_msgs/msg/InnovizScan.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +InnovizPacket[] packets diff --git a/nebula_messages/innoviz_msgs/package.xml b/nebula_messages/innoviz_msgs/package.xml new file mode 100644 index 000000000..13ddbbf89 --- /dev/null +++ b/nebula_messages/innoviz_msgs/package.xml @@ -0,0 +1,24 @@ + + + innoviz_msgs + 0.0.0 + + ROS message definition for the Innoviz LiDAR sensors. + + Apache 2 + invz + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 6848b9dec..a258db4d5 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -90,6 +90,25 @@ rclcpp_components_register_node(velodyne_driver_ros_wrapper EXECUTABLE velodyne_driver_ros_wrapper_node ) +## Innoviz +# Hw Interface +ament_auto_add_library(innoviz_hw_ros_wrapper SHARED + src/innoviz/innoviz_hw_interface_ros_wrapper.cpp + ) +rclcpp_components_register_node(innoviz_hw_ros_wrapper + PLUGIN "InnovizHwInterfaceRosWrapper" + EXECUTABLE innoviz_hw_ros_wrapper_node + ) + +# DriverDecoder +ament_auto_add_library(innoviz_driver_ros_wrapper SHARED + src/innoviz/innoviz_decoder_ros_wrapper.cpp + ) +rclcpp_components_register_node(innoviz_driver_ros_wrapper + PLUGIN "InnovizDriverRosWrapper" + EXECUTABLE innoviz_driver_ros_wrapper_node + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_ros/config/innoviz/InnovizTwoCondor.yaml b/nebula_ros/config/innoviz/InnovizTwoCondor.yaml new file mode 100644 index 000000000..0ef244d28 --- /dev/null +++ b/nebula_ros/config/innoviz/InnovizTwoCondor.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + sensor_model: "InnovizTwoCondor" + sensor_ip: "192.168.0.5" + host_ip: "192.168.0.2" + data_port: 9210 + return_mode: "SingleStrongest" + frame_id: "innoviz" + setup_sensor: False + online: True + filter_artifacts: True diff --git a/nebula_ros/config/innoviz/InnovizTwoRaven.yaml b/nebula_ros/config/innoviz/InnovizTwoRaven.yaml new file mode 100644 index 000000000..da7975163 --- /dev/null +++ b/nebula_ros/config/innoviz/InnovizTwoRaven.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + sensor_model: "InnovizTwoRaven" + sensor_ip: "192.168.0.5" + host_ip: "192.168.0.2" + data_port: 9210 + return_mode: "SingleStrongest" + frame_id: "innoviz" + setup_sensor: False + online: True + filter_artifacts: True diff --git a/nebula_ros/include/nebula_ros/innoviz/innoviz_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/innoviz/innoviz_decoder_ros_wrapper.hpp new file mode 100644 index 000000000..9c9800d3f --- /dev/null +++ b/nebula_ros/include/nebula_ros/innoviz/innoviz_decoder_ros_wrapper.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include "nebula_common/innoviz/innoviz_common.hpp" +#include "nebula_decoders/nebula_decoders_innoviz/innoviz_driver.hpp" +#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" + +#include + +#include "innoviz_msgs/msg/innoviz_packet.hpp" +#include "innoviz_msgs/msg/innoviz_scan.hpp" + + +namespace nebula +{ +namespace ros +{ + +/// @brief ROS wrapper of Innoviz driver +class InnovizDriverRosWrapper : public rclcpp::Node, NebulaDriverRosWrapperBase +{ +public: + InnovizDriverRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Returns the Status of the driver + Status GetStatus(); + +private: + + /// @brief Convers ROS parameters to InnovizSensorConfiguration + /// @param sensor_configuration Innoviz sensor configuration toe fill + /// @return Resulting status + Status GetParameters(drivers::InnovizSensorConfiguration & sensor_configuration); + + /// @brief Initializes the Innoviz Driver + /// @param sensor_configuration Sensor configuration for the driver to initialize + Status InitializeDriver(std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) override; + + /// @brief Callback for the InnovizScan subscriber + /// @param scan_msg The receive InnovizScan to handle + void ReceiveScanMsgCallback(const innoviz_msgs::msg::InnovizScan::SharedPtr scan_msg); + +private: + std::shared_ptr driver_ptr_; + Status wrapper_status_; + rclcpp::Subscription::SharedPtr innoviz_scan_sub_; + rclcpp::Publisher::SharedPtr nebula_points_pub_; + + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/innoviz/innoviz_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/innoviz/innoviz_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..c9892d7df --- /dev/null +++ b/nebula_ros/include/nebula_ros/innoviz/innoviz_hw_interface_ros_wrapper.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include "nebula_common/innoviz/innoviz_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_innoviz/innoviz_hw_interface.hpp" +#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" + +#include + +#include "innoviz_msgs/msg/innoviz_packet.hpp" +#include "innoviz_msgs/msg/innoviz_scan.hpp" + +namespace nebula +{ +namespace ros +{ + +class InnovizHwInterfaceRosWrapper : public rclcpp::Node, NebulaHwInterfaceWrapperBase +{ +public: + InnovizHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + + /// @brief Get configurations from ROS parameters + /// @param sensorConfiguration Ouptut SensorConfiguration + /// @return Resulting status + Status GetParameters(drivers::InnovizSensorConfiguration & sensorConfiguration); + +private: + Status InitializeHwInterface(const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback for receiving InnovizScan + /// @param scan_buffer Received InnovizScan + void ReceiveScanDataCallback(std::unique_ptr scan_buffer); + + +private: + drivers::InnovizHwInterface hw_interface_; + drivers::InnovizSensorConfiguration sensor_configuration_; + Status interface_status_; + + rclcpp::Publisher::SharedPtr innoviz_scan_pub_; +}; + +} +} \ No newline at end of file diff --git a/nebula_ros/launch/innoviz_launch_raven.xml b/nebula_ros/launch/innoviz_launch_raven.xml new file mode 100644 index 000000000..ab16c3a5f --- /dev/null +++ b/nebula_ros/launch/innoviz_launch_raven.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f9c30f47d..d6e78a81b 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -20,6 +20,8 @@ def get_lidar_make(sensor_name): return "Hesai", ".csv" elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: return "Velodyne", ".yaml" + elif sensor_name[:7].lower() == "innoviz": + return "Innoviz", ".csv" return "unrecognized_sensor_model" diff --git a/nebula_ros/src/innoviz/innoviz_decoder_ros_wrapper.cpp b/nebula_ros/src/innoviz/innoviz_decoder_ros_wrapper.cpp new file mode 100644 index 000000000..406387f4e --- /dev/null +++ b/nebula_ros/src/innoviz/innoviz_decoder_ros_wrapper.cpp @@ -0,0 +1,148 @@ + +#include "nebula_ros/innoviz/innoviz_decoder_ros_wrapper.hpp" + +#include +#include + +namespace nebula +{ +namespace ros +{ + +InnovizDriverRosWrapper::InnovizDriverRosWrapper(const rclcpp::NodeOptions& options) + : rclcpp::Node("innoviz_driver_ros_wrapper", options) +{ + drivers::InnovizSensorConfiguration sensorConfiguration; + + wrapper_status_ = GetParameters(sensorConfiguration); + + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + sensor_cfg_ptr_ = std::make_shared(sensorConfiguration); + auto calib_ptr = std::make_shared(); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + wrapper_status_ = InitializeDriver(std::const_pointer_cast(sensor_cfg_ptr_), calib_ptr); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + + auto innovizScancallbackFunction = std::bind(&InnovizDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1); + + innoviz_scan_sub_ = create_subscription( + "innoviz_packets", + rclcpp::SensorDataQoS(), + innovizScancallbackFunction); + + nebula_points_pub_ = this->create_publisher( + "innoviz_points", + rclcpp::SensorDataQoS()); +} + +Status InnovizDriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr /*calibration_configuration*/) +{ + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration)); + return driver_ptr_->GetStatus(); +} + +Status InnovizDriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} + + +void InnovizDriverRosWrapper::ReceiveScanMsgCallback(const innoviz_msgs::msg::InnovizScan::SharedPtr scan_msg) +{ + nebula::drivers::NebulaPointCloudPtr pointcloud = driver_ptr_->ConvertScanToPointcloud(scan_msg); + //pointcloud->header.stamp; + + int subCount = nebula_points_pub_->get_subscription_count() + nebula_points_pub_->get_intra_process_subscription_count(); + if (subCount > 0) + { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + + ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id; + nebula_points_pub_->publish(std::move(ros_pc_msg_ptr)); + } +} + + +Status InnovizDriverRosWrapper::GetParameters(drivers::InnovizSensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", ""); + + std::string sensorModelString = this->get_parameter("sensor_model").as_string(); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensorModelString); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "innoviz", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("min_range", 1.2, descriptor); + sensor_configuration.min_range = this->get_parameter("min_range").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("max_range", 250., descriptor); + sensor_configuration.max_range = this->get_parameter("max_range").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("min_confidence", 21, descriptor); + sensor_configuration.min_confidence = this->get_parameter("min_confidence").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("filter_artifacts", true, descriptor); + sensor_configuration.filter_artifacts = this->get_parameter("filter_artifacts").as_bool(); + } + + if(sensor_configuration.sensor_model != nebula::drivers::SensorModel::INNOVIZ_TWO_CONDOR && + sensor_configuration.sensor_model != nebula::drivers::SensorModel::INNOVIZ_TWO_RAVEN) + { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "Sensor model: " << sensor_configuration.sensor_model); + + return Status::OK; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(InnovizDriverRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/innoviz/innoviz_hw_interface_ros_wrapper.cpp b/nebula_ros/src/innoviz/innoviz_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..4ae58f96e --- /dev/null +++ b/nebula_ros/src/innoviz/innoviz_hw_interface_ros_wrapper.cpp @@ -0,0 +1,127 @@ + +#include "nebula_ros/innoviz/innoviz_hw_interface_ros_wrapper.hpp" +#include + +namespace nebula +{ +namespace ros +{ + + InnovizHwInterfaceRosWrapper::InnovizHwInterfaceRosWrapper(const rclcpp::NodeOptions & options) + : rclcpp::Node("innoviz_hw_interface_ros_wrapper", options), hw_interface_() + { + interface_status_ = GetParameters(sensor_configuration_); + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + + RCLCPP_INFO_STREAM(this->get_logger(), "Initialize sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration(std::static_pointer_cast(sensor_cfg_ptr)); + + auto scanCallbackFunction = std::bind(&InnovizHwInterfaceRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1); + // register scan callback and publisher + hw_interface_.RegisterScanCallback(scanCallbackFunction); + innoviz_scan_pub_ = this->create_publisher( + "innoviz_packets", + rclcpp::SensorDataQoS(rclcpp::KeepLast(10)).best_effort().durability_volatile()); + + auto status = StreamStart(); + if (status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "UDP Driver Started"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), status); + } + } + + Status InnovizHwInterfaceRosWrapper::StreamStart() + { + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.CloudInterfaceStart(); + } + return interface_status_; + } + + Status InnovizHwInterfaceRosWrapper::StreamStop() { return Status::OK; } + Status InnovizHwInterfaceRosWrapper::Shutdown() { return Status::OK; } + + + void InnovizHwInterfaceRosWrapper::ReceiveScanDataCallback(std::unique_ptr scan_buffer) + { + scan_buffer->header.frame_id = sensor_configuration_.frame_id; + //TODO: Add timestamp?? + innoviz_scan_pub_->publish(*scan_buffer); + } + + Status InnovizHwInterfaceRosWrapper::InitializeHwInterface(const drivers::SensorConfigurationBase & /*sensor_configuration*/) + { + return Status::OK; + } + + + Status InnovizHwInterfaceRosWrapper::GetParameters(drivers::InnovizSensorConfiguration & sensor_configuration) + { + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", ""); + std::string sensorModelString = this->get_parameter("sensor_model").as_string(); + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensorModelString); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", "192.168.0.2", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", "192.168.0.5", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "innoviz", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", 9210, descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + + if(sensor_configuration.sensor_model != nebula::drivers::SensorModel::INNOVIZ_TWO_CONDOR && + sensor_configuration.sensor_model != nebula::drivers::SensorModel::INNOVIZ_TWO_RAVEN) + { + return Status::INVALID_SENSOR_MODEL; + } + + return Status::OK; + } + +RCLCPP_COMPONENTS_REGISTER_NODE(InnovizHwInterfaceRosWrapper) +} +} diff --git a/nebula_sensor_driver/package.xml b/nebula_sensor_driver/package.xml index fa656a543..e5b39f431 100644 --- a/nebula_sensor_driver/package.xml +++ b/nebula_sensor_driver/package.xml @@ -18,6 +18,7 @@ nebula_tests pandar_msgs velodyne_msgs + innoviz_msgs ament_cmake diff --git a/nebula_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index bd58aa6fc..19e2336c4 100644 --- a/nebula_tests/CMakeLists.txt +++ b/nebula_tests/CMakeLists.txt @@ -50,11 +50,13 @@ if(BUILD_TESTING) rosbag2_cpp pandar_msgs velodyne_msgs + innoviz_msgs ) add_subdirectory(hesai) add_subdirectory(velodyne) + add_subdirectory(innoviz) endif() diff --git a/nebula_tests/data/innoviz/raven/1693484175465116040.pcd b/nebula_tests/data/innoviz/raven/1693484175465116040.pcd new file mode 100644 index 000000000..fa00121f4 Binary files /dev/null and b/nebula_tests/data/innoviz/raven/1693484175465116040.pcd differ diff --git a/nebula_tests/data/innoviz/raven/1693484175465116040/1693484175465116040_0.db3 b/nebula_tests/data/innoviz/raven/1693484175465116040/1693484175465116040_0.db3 new file mode 100644 index 000000000..261b42ad5 Binary files /dev/null and b/nebula_tests/data/innoviz/raven/1693484175465116040/1693484175465116040_0.db3 differ diff --git a/nebula_tests/data/innoviz/raven/1693484175465116040/metadata.yaml b/nebula_tests/data/innoviz/raven/1693484175465116040/metadata.yaml new file mode 100644 index 000000000..d4ace3f03 --- /dev/null +++ b/nebula_tests/data/innoviz/raven/1693484175465116040/metadata.yaml @@ -0,0 +1,19 @@ +rosbag2_bagfile_information: + version: 4 + storage_identifier: sqlite3 + relative_file_paths: + - 1693484175465116040_0.db3 + duration: + nanoseconds: 194975470 + starting_time: + nanoseconds_since_epoch: 1693484175465116040 + message_count: 2 + topics_with_message_count: + - topic_metadata: + name: /innoviz_packets + type: innoviz_msgs/msg/InnovizScan + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 10\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: "" diff --git a/nebula_tests/innoviz/CMakeLists.txt b/nebula_tests/innoviz/CMakeLists.txt new file mode 100644 index 000000000..c3f9eec02 --- /dev/null +++ b/nebula_tests/innoviz/CMakeLists.txt @@ -0,0 +1,20 @@ +# Innoviz Raven +ament_auto_add_library(innoviz_ros_decoder_test_raven SHARED + innoviz_ros_decoder_test_raven.cpp + ) +target_link_libraries(innoviz_ros_decoder_test_raven ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) + +ament_add_gtest(innoviz_ros_decoder_test_main_raven + innoviz_ros_decoder_test_main_raven.cpp + ) +ament_target_dependencies(innoviz_ros_decoder_test_main_raven + ${NEBULA_TEST_DEPENDENCIES} + ) +target_include_directories(innoviz_ros_decoder_test_main_raven PUBLIC + ${PROJECT_SOURCE_DIR}/src/innoviz + include + ) +target_link_libraries(innoviz_ros_decoder_test_main_raven + ${PCL_LIBRARIES} + innoviz_ros_decoder_test_raven + ) \ No newline at end of file diff --git a/nebula_tests/innoviz/innoviz_ros_decoder_test_main_raven.cpp b/nebula_tests/innoviz/innoviz_ros_decoder_test_main_raven.cpp new file mode 100644 index 000000000..1317d7b62 --- /dev/null +++ b/nebula_tests/innoviz/innoviz_ros_decoder_test_main_raven.cpp @@ -0,0 +1,47 @@ +#include "innoviz_ros_decoder_test_raven.hpp" + +#include + +#include + +#include + +std::shared_ptr innoviz_driver; + +TEST(TestDecoder, TestPcd) +{ + std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; + innoviz_driver->ReadBag(); +} + +int main(int argc, char * argv[]) +{ + std::cout << "innoviz_ros_decoder_test_raven.cpp" << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + std::string node_name = "nebula_innoviz_decoder_test"; + + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + + innoviz_driver = std::make_shared(options, node_name); + exec.add_node(innoviz_driver->get_node_base_interface()); + + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); + nebula::Status driver_status = innoviz_driver->GetStatus(); + int result = 0; + if (driver_status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); + // innoviz_driver->ReadBag(); + result = RUN_ALL_TESTS(); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); + } + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); + innoviz_driver.reset(); + rclcpp::shutdown(); + + return result; +} diff --git a/nebula_tests/innoviz/innoviz_ros_decoder_test_raven.cpp b/nebula_tests/innoviz/innoviz_ros_decoder_test_raven.cpp new file mode 100644 index 000000000..866038174 --- /dev/null +++ b/nebula_tests/innoviz/innoviz_ros_decoder_test_raven.cpp @@ -0,0 +1,267 @@ +#include "innoviz_ros_decoder_test_raven.hpp" + +#include "rclcpp/serialization.hpp" +#include "rclcpp/serialized_message.hpp" +#include "rcpputils/filesystem_helper.hpp" +#include "rcutils/time.h" +#include "rosbag2_cpp/reader.hpp" +#include "rosbag2_cpp/readers/sequential_reader.hpp" +#include "rosbag2_cpp/writer.hpp" +#include "rosbag2_cpp/writers/sequential_writer.hpp" +#include "rosbag2_storage/storage_options.hpp" + +#include + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +InnovizRosDecoderTest::InnovizRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name) +: rclcpp::Node(node_name, options) +{ + drivers::CalibrationConfigurationBase calibration_configuration; + drivers::InnovizSensorConfiguration sensor_configuration; + + wrapper_status_ = + GetParameters(sensor_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + calibration_cfg_ptr_ = + std::make_shared(calibration_configuration); + + sensor_cfg_ptr_ = std::make_shared(sensor_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + wrapper_status_ = InitializeDriver( + std::const_pointer_cast(sensor_cfg_ptr_), + calibration_cfg_ptr_); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); +} + +Status InnovizRosDecoderTest::InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr /*calibration_configuration*/) +{ + // driver should be initialized here with proper decoder + driver_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration)); + return driver_ptr_->GetStatus(); +} + +Status InnovizRosDecoderTest::GetStatus() { return wrapper_status_; } + +Status InnovizRosDecoderTest::GetParameters( + drivers::InnovizSensorConfiguration & sensor_configuration) +{ + std::filesystem::path calib_dir = + _SRC_CALIBRATION_DIR_PATH; // variable defined in CMakeLists.txt; + calib_dir /= "innoviz"; + std::filesystem::path bag_root_dir = + _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; + bag_root_dir /= "innoviz"; + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", "InnovizTwoRaven"); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("return_mode", "SingleStrongest", descriptor); + sensor_configuration.return_mode = nebula::drivers::ReturnModeFromString( + this->get_parameter("return_mode").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "innoviz", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter( + "bag_path", (bag_root_dir / "raven" / "1693484175465116040").string(), descriptor); + bag_path = this->get_parameter("bag_path").as_string(); + std::cout << bag_path << std::endl; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("storage_id", "sqlite3", descriptor); + storage_id = this->get_parameter("storage_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("format", "cdr", descriptor); + format = this->get_parameter("format").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("target_topic", "/innoviz_packets", descriptor); + target_topic = this->get_parameter("target_topic").as_string(); + } + + if (sensor_configuration.sensor_model != nebula::drivers::SensorModel::INNOVIZ_TWO_RAVEN) { + return Status::INVALID_SENSOR_MODEL; + } + if (sensor_configuration.return_mode != nebula::drivers::ReturnMode::SINGLE_STRONGEST && + sensor_configuration.return_mode != nebula::drivers::ReturnMode::FIRST) { + return Status::INVALID_ECHO_MODE; + } + + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void printPCD(nebula::drivers::NebulaPointCloudPtr pp) +{ + for (auto p : pp->points) { + std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " + << p.channel << ", " << p.azimuth << ", " << p.return_type << ", " << p.time_stamp + << std::endl; + } +} + +void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) +{ + EXPECT_EQ(pp1->points.size(), pp2->points.size()); + for (uint32_t i = 0; i < pp1->points.size(); i++) { + auto p1 = pp1->points[i]; + auto p2 = pp2->points[i]; + EXPECT_FLOAT_EQ(p1.x, p2.x); + EXPECT_FLOAT_EQ(p1.y, p2.y); + EXPECT_FLOAT_EQ(p1.z, p2.z); + EXPECT_FLOAT_EQ(p1.intensity, p2.intensity); + EXPECT_EQ(p1.channel, p2.channel); + EXPECT_FLOAT_EQ(p1.azimuth, p2.azimuth); + EXPECT_EQ(p1.return_type, p2.return_type); + EXPECT_DOUBLE_EQ(p1.time_stamp, p2.time_stamp); + } +} + +void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud::Ptr pp2) +{ + EXPECT_EQ(pp1->points.size(), pp2->points.size()); + for (uint32_t i = 0; i < pp1->points.size(); i++) { + auto p1 = pp1->points[i]; + auto p2 = pp2->points[i]; + EXPECT_FLOAT_EQ(p1.x, p2.x); + EXPECT_FLOAT_EQ(p1.y, p2.y); + EXPECT_FLOAT_EQ(p1.z, p2.z); + } +} + +void InnovizRosDecoderTest::ReadBag() +{ + rosbag2_storage::StorageOptions storage_options; + rosbag2_cpp::ConverterOptions converter_options; + + std::cout << bag_path << std::endl; + std::cout << storage_id << std::endl; + std::cout << format << std::endl; + std::cout << target_topic << std::endl; + + auto target_topic_name = target_topic; + if (target_topic_name.substr(0, 1) == "/") { + target_topic_name = target_topic_name.substr(1); + } + target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); + + pcl::PCDReader pcd_reader; + + rcpputils::fs::path bag_dir(bag_path); + rcpputils::fs::path pcd_dir = bag_dir.parent_path(); + int check_cnt = 0; + + storage_options.uri = bag_path; + storage_options.storage_id = storage_id; + converter_options.output_serialization_format = format; //"cdr"; + rclcpp::Serialization serialization; + nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); + // nebula::drivers::NebulaPointCloudPtr ref_pointcloud(new nebula::drivers::NebulaPointCloud); + pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); + { + rosbag2_cpp::Reader bag_reader(std::make_unique()); + bag_reader.open(storage_options, converter_options); + while (bag_reader.has_next()) { + auto bag_message = bag_reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name == target_topic) { + innoviz_msgs::msg::InnovizScan extracted_msg; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + auto extracted_msg_ptr = std::make_shared(extracted_msg); + pointcloud = driver_ptr_->ConvertScanToPointcloud(extracted_msg_ptr); + std::cout << "Num of pixels = " << pointcloud->size() << std::endl; + + // There are very rare cases where has_scanned_ does not become true, but it is not known + // whether it is because of decoder or deserialize_message. + if (!pointcloud) continue; + + auto fn = std::to_string(bag_message->time_stamp) + ".pcd"; + + auto target_pcd_path = (pcd_dir / fn); + std::cout << target_pcd_path << std::endl; + if (target_pcd_path.exists()) { + std::cout << "exists: " << target_pcd_path << std::endl; + auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); + std::cout << rt << " loaded: " << target_pcd_path << std::endl; + checkPCDs(pointcloud, ref_pointcloud); + check_cnt++; + // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); + ref_pointcloud.reset(new pcl::PointCloud); + } + pointcloud.reset(new nebula::drivers::NebulaPointCloud); + } + } + EXPECT_GT(check_cnt, 0); + // close on scope exit + } +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/innoviz/innoviz_ros_decoder_test_raven.hpp b/nebula_tests/innoviz/innoviz_ros_decoder_test_raven.hpp new file mode 100644 index 000000000..3a19f39db --- /dev/null +++ b/nebula_tests/innoviz/innoviz_ros_decoder_test_raven.hpp @@ -0,0 +1,68 @@ +#ifndef NEBULA_HesaiRosDecoderTest64_H +#define NEBULA_HesaiRosDecoderTest64_H + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_decoders/nebula_decoders_innoviz/innoviz_driver.hpp" +#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" + +#include +#include +#include + +#include "innoviz_msgs/msg/innoviz_packet.hpp" +#include "innoviz_msgs/msg/innoviz_scan.hpp" + +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Testing Innoviz decoder +class InnovizRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBase //, testing::Test +{ + std::shared_ptr driver_ptr_; + Status wrapper_status_; + + std::shared_ptr calibration_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; + + /// @brief Initializing ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver + /// @return Resulting status + Status InitializeDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) override; + + + /// @brief Get configurations + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters(drivers::InnovizSensorConfiguration & sensor_configuration); + + +public: + InnovizRosDecoderTest(const rclcpp::NodeOptions & options, const std::string & node_name); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Read the specified bag file and compare the constructed point clouds with the + /// corresponding PCD files + void ReadBag(); + +private: + std::string bag_path; + std::string storage_id; + std::string format; + std::string target_topic; + std::string correction_file_path; +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_HesaiRosDecoderTest64_H