diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index cf1eb0841..0fdd4612e 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -327,7 +327,9 @@ enum class SensorModel { ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, - CONTINENTAL_ARS548 + CONTINENTAL_ARS548, + SEYOND_FALCON_KINETIC, + SEYOND_ROBIN_W }; /// @brief not used? @@ -440,6 +442,12 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::CONTINENTAL_ARS548: os << "ARS548"; break; + case SensorModel::SEYOND_FALCON_KINETIC: + os << "SEYOND_FALCON_KINETIC"; + break; + case SensorModel::SEYOND_ROBIN_W: + os << "SEYOND_ROBIN_W"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -544,6 +552,9 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; + // Seyond + if (sensor_model == "Falcon") return SensorModel::SEYOND_FALCON_KINETIC; + if (sensor_model == "RobinW") return SensorModel::SEYOND_ROBIN_W; return SensorModel::UNKNOWN; } @@ -593,6 +604,10 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) return "Bpearl_V4"; case SensorModel::CONTINENTAL_ARS548: return "ARS548"; + case SensorModel::SEYOND_FALCON_KINETIC: + return "Falcon"; + case SensorModel::SEYOND_ROBIN_W: + return "RoninW"; default: return "UNKNOWN"; } diff --git a/nebula_common/include/nebula_common/seyond/seyond_common.hpp b/nebula_common/include/nebula_common/seyond/seyond_common.hpp new file mode 100755 index 000000000..6d6ea3cba --- /dev/null +++ b/nebula_common/include/nebula_common/seyond/seyond_common.hpp @@ -0,0 +1,108 @@ +#ifndef NEBULA_SEYOND_COMMON_H +#define NEBULA_SEYOND_COMMON_H + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" + +#include +#include +#include +#include +#include +namespace nebula +{ +namespace drivers +{ +/// @brief struct for Seyond sensor configuration +struct SeyondSensorConfiguration : public LidarConfigurationBase +{ + uint16_t gnss_port{}; + double scan_phase{}; + double dual_return_distance_threshold{}; + uint16_t rotation_speed; + uint16_t cloud_min_angle; + uint16_t cloud_max_angle; + PtpProfile ptp_profile; + uint8_t ptp_domain; + PtpTransportType ptp_transport_type; + PtpSwitchType ptp_switch_type; +}; +/// @brief Convert SeyondSensorConfiguration to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, SeyondSensorConfiguration const & arg) +{ + os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port + << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed + << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle + << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold + << ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain) + << ", PtpTransportType:" << arg.ptp_transport_type + << ", PtpSwitchType:" << arg.ptp_switch_type; + return os; +} + +struct SeyondCalibrationConfigurationBase : public CalibrationConfigurationBase +{ + +}; + +/// @brief struct for Seyond calibration configuration +struct SeyondCalibrationConfiguration : public SeyondCalibrationConfigurationBase +{ + std::map elev_angle_map; + std::map azimuth_offset_map; +}; + +/* + + + + + + +*/ +/* + + + + + + + +*/ + +/// @brief Convert return mode name to ReturnMode enum (Seyond-specific ReturnModeFromString) +/// @param return_mode Return mode name (Upper and lower case letters must match) +/// @param sensor_model Model for correct conversion +/// @return Corresponding ReturnMode +inline ReturnMode ReturnModeFromStringSeyond( + const std::string & return_mode, const SensorModel & sensor_model) +{ + return ReturnMode::UNKNOWN; +} + +/// @brief Convert return mode number to ReturnMode enum +/// @param return_mode Return mode number from the hardware response +/// @param sensor_model Model for correct conversion +/// @return Corresponding ReturnMode +inline ReturnMode ReturnModeFromIntSeyond(const int return_mode, const SensorModel & sensor_model) +{ + return ReturnMode::UNKNOWN; +} + +/// @brief Convert ReturnMode enum to return mode number +/// @param return_mode target ReturnMode +/// @param sensor_model Model for correct conversion +/// @return Corresponding return mode number for the hardware +inline int IntFromReturnModeSeyond(const ReturnMode return_mode, const SensorModel & sensor_model) +{ + + return -1; +} + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_SEYOND_COMMON_H \ No newline at end of file diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 31385de6f..fc7f9262d 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -30,6 +30,12 @@ include_directories( ) # Lidar Decoders + +# Hesai +ament_auto_add_library(nebula_decoders_seyond SHARED + src/nebula_decoders_seyond/seyond_driver.cpp + ) + # Hesai ament_auto_add_library(nebula_decoders_hesai SHARED src/nebula_decoders_hesai/hesai_driver.cpp diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/falcon.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/falcon.hpp new file mode 100644 index 000000000..91099dd46 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/falcon.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp" +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_sensor.hpp" + +namespace nebula +{ +namespace drivers +{ + +namespace seyond_packet +{ + +#pragma pack(push, 1) + +struct PacketFalcon : public PacketBase<8, 32, 2, 100> +{ + +}; + +#pragma pack(pop) + +} // namespace seyond_packet + +class Falcon : public SeyondSensor +{ + + +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/robin_w.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/robin_w.hpp new file mode 100644 index 000000000..7188088c5 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/robin_w.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp" +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_sensor.hpp" + +namespace nebula +{ +namespace drivers +{ + +namespace seyond_packet +{ + +#pragma pack(push, 1) + +struct PacketRobinW : public PacketBase<8, 32, 2, 100> +{ + +}; + +#pragma pack(pop) + +} // namespace seyond_packet + +class RobinW : public SeyondSensor +{ + + +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_decoder.hpp new file mode 100755 index 000000000..34d5039e8 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_decoder.hpp @@ -0,0 +1,111 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp" +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_scan_decoder.hpp" + +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" + +namespace nebula +{ +namespace drivers +{ + +template +class SeyondDecoder : public SeyondScanDecoder +{ +protected: + /// @brief Configuration for this decoder + const std::shared_ptr sensor_configuration_; + + /// @brief The sensor definition, used for return mode and time offset handling + SensorT sensor_{}; + + /// @brief Decodes azimuth/elevation angles given calibration/correction data + typename SensorT::angle_corrector_t angle_corrector_; + + /// @brief The point cloud new points get added to + NebulaPointCloudPtr decode_pc_; + /// @brief The point cloud that is returned when a scan is complete + NebulaPointCloudPtr output_pc_; + + /// @brief The last decoded packet + typename SensorT::packet_t packet_; + /// @brief The last azimuth processed + int last_phase_ = -1; // Dummy value to signal last_phase_ has not been set yet + /// @brief The timestamp of the last completed scan in nanoseconds + uint64_t output_scan_timestamp_ns_ = 0; + /// @brief The timestamp of the scan currently in progress + uint64_t decode_scan_timestamp_ns_ = 0; + /// @brief Whether a full scan has been processed + bool has_scanned_ = false; + + rclcpp::Logger logger_; + + /// @brief For each channel, its firing offset relative to the block in nanoseconds + std::array channel_firing_offset_ns_; + /// @brief For each return mode, the firing offset of each block relative to its packet in + /// nanoseconds + std::array, SensorT::packet_t::MAX_RETURNS> + block_firing_offset_ns_; + + /// @brief Validates and parse NebulaPacket. Currently only checks size, not checksums etc. + /// @param packet The incoming NebulaPacket + /// @return Whether the packet was parsed successfully + bool parsePacket(const std::vector & packet) + { + if (packet.size() < sizeof(typename SensorT::packet_t)) { + RCLCPP_ERROR_STREAM( + logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:" + << sizeof(typename SensorT::packet_t)); + return false; + } + if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) { + // FIXME(mojomex) do validation? + // RCLCPP_DEBUG(logger_, "Packet parsed successfully"); + return true; + } + + RCLCPP_ERROR(logger_, "Packet memcopy failed"); + return false; + } + +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this decoder + /// @param correction_data Calibration data for this decoder + explicit SeyondDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & correction_data) + : sensor_configuration_(sensor_configuration), + angle_corrector_(correction_data), + logger_(rclcpp::get_logger("SeyondDecoder")) + { + logger_.set_level(rclcpp::Logger::Level::Debug); + RCLCPP_INFO_STREAM(logger_, *sensor_configuration_); + + decode_pc_.reset(new NebulaPointCloud); + output_pc_.reset(new NebulaPointCloud); + + decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + } + + int unpack(const std::vector & packet) override + { + return -1; + } + + bool hasScanned() override { return has_scanned_; } + + std::tuple getPointcloud() override + { + double scan_timestamp_s = static_cast(output_scan_timestamp_ns_) * 1e-9; + return std::make_pair(output_pc_, scan_timestamp_s); + } +}; + +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp new file mode 100755 index 000000000..ae52ca114 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp @@ -0,0 +1,106 @@ +#pragma once + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace seyond_packet +{ + +// FIXME(mojomex) This is a workaround for the compiler being pedantic about casting `enum class`s +// to their underlying type +namespace return_mode +{ +enum ReturnMode { + SINGLE_FIRST = 0x33, + SINGLE_SECOND = 0x34, + SINGLE_STRONGEST = 0x37, + SINGLE_LAST = 0x38, + DUAL_LAST_STRONGEST = 0x39, + DUAL_FIRST_SECOND = 0x3a, + DUAL_FIRST_LAST = 0x3b, + DUAL_FIRST_STRONGEST = 0x3c, + TRIPLE_FIRST_LAST_STRONGEST = 0x3d, + DUAL_STRONGEST_SECONDSTRONGEST = 0x3, +}; +} // namespace return_mode + +#pragma pack(push, 1) + +/// @brief DateTime struct for Seyond packets +/// @tparam YearOffset like std::tm, the Seyond format has a year offset that is applied to the raw +/// year value. For most protocol versions it is 1900 (like std::tm), for some it is 2000. +template +struct DateTime +{ + /// @brief Year - YearOffset (e.g. for YearOffset=1900 value 100 corresponds to year 2000) + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + + /// @brief Get seconds since epoch + /// @return Whole seconds since epoch + uint64_t get_seconds() const + { + std::tm tm{}; + tm.tm_year = year - 1900 + YearOffset; + tm.tm_mon = month - 1; // starts from 0 in C + tm.tm_mday = day; + tm.tm_hour = hour; + tm.tm_min = minute; + tm.tm_sec = second; + return timegm(&tm); + } +}; + +struct SecondsSinceEpoch +{ + uint8_t zero; + /// @brief Seconds since epoch, in big-endian format + uint8_t seconds[5]; + + /// @brief Get seconds since epoch + /// @return Whole seconds since epoch + uint64_t get_seconds() const + { + uint64_t seconds = 0; + for (int i = 0; i < 5; ++i) { + seconds = (seconds << 8) | this->seconds[i]; + } + return seconds; + } +}; + +template +struct Body +{ + typedef BlockT block_t; + BlockT blocks[BlockN]; +}; + +/// @brief Base struct for all Seyond packets. This struct is not allowed to have any non-static +/// members, otherwise memory layout is not guaranteed for the derived structs. +/// @tparam nBlocks The number of blocks in the packet +/// @tparam nChannels The number of channels per block +/// @tparam maxReturns The maximum number of returns, e.g. 2 for dual return +/// @tparam degreeSubdivisions The resolution of the azimuth angle in the packet, e.g. 100 if packet +/// azimuth is given in 1/100th of a degree +template +struct PacketBase +{ + static constexpr size_t N_BLOCKS = nBlocks; + static constexpr size_t N_CHANNELS = nChannels; + static constexpr size_t MAX_RETURNS = maxReturns; + static constexpr size_t DEGREE_SUBDIVISIONS = degreeSubdivisions; +}; + +} // namespace seyond_packet +} // namespace drivers +} // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_scan_decoder.hpp new file mode 100755 index 000000000..8f647c109 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_scan_decoder.hpp @@ -0,0 +1,43 @@ +#ifndef NEBULA_WS_HESAI_SCAN_DECODER_HPP +#define NEBULA_WS_HESAI_SCAN_DECODER_HPP + +#include "nebula_common/seyond/seyond_common.hpp" +#include "nebula_common/point_types.hpp" + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ +/// @brief Base class for Seyond LiDAR decoder +class SeyondScanDecoder +{ +public: + SeyondScanDecoder(SeyondScanDecoder && c) = delete; + SeyondScanDecoder & operator=(SeyondScanDecoder && c) = delete; + SeyondScanDecoder(const SeyondScanDecoder & c) = delete; + SeyondScanDecoder & operator=(const SeyondScanDecoder & c) = delete; + + virtual ~SeyondScanDecoder() = default; + SeyondScanDecoder() = default; + + /// @brief Parses PandarPacket and add its points to the point cloud + /// @param packet The incoming PandarPacket + /// @return The last azimuth processed + virtual int unpack(const std::vector & packet) = 0; + + /// @brief Indicates whether one full scan is ready + /// @return Whether a scan is ready + virtual bool hasScanned() = 0; + + /// @brief Returns the point cloud and timestamp of the last scan + /// @return A tuple of point cloud and timestamp in nanoseconds + virtual std::tuple getPointcloud() = 0; +}; +} // namespace drivers +} // namespace nebula +#endif // NEBULA_WS_HESAI_SCAN_DECODER_HPP diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_sensor.hpp new file mode 100644 index 000000000..88c0a4125 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/seyond_sensor.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include "nebula_common/nebula_common.hpp" +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_packet.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ + +/// @brief Base class for all sensor definitions +/// @tparam PacketT The packet type of the sensor +template +class SeyondSensor +{ +private: + +public: + typedef PacketT packet_t; + + SeyondSensor() = default; + virtual ~SeyondSensor() = default; +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/seyond_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/seyond_driver.hpp new file mode 100755 index 000000000..bb6a58090 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/seyond_driver.hpp @@ -0,0 +1,70 @@ +#ifndef NEBULA_SEYOND_DRIVER_H +#define NEBULA_SEYOND_DRIVER_H + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_common/point_types.hpp" +#include "nebula_common/seyond/seyond_common.hpp" +#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" +#include "nebula_decoders/nebula_decoders_seyond/decoders/seyond_decoder.hpp" + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" + +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +/// @brief Seyond driver +class SeyondDriver +{ +private: + /// @brief Current driver status + Status driver_status_; + /// @brief Decoder according to the model + std::shared_ptr scan_decoder_; + + template + std::shared_ptr InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); + +public: + SeyondDriver() = delete; + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver (either + explicit SeyondDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Setting CalibrationConfiguration (not used) + /// @param calibration_configuration + /// @return Resulting status + Status SetCalibrationConfiguration( + const SeyondCalibrationConfigurationBase & calibration_configuration); + + /// @brief Convert NebulaPackets message to point cloud + /// @param nebula_packets Message + /// @return tuple of Point cloud and timestamp + std::tuple ParseCloudPacket( + const std::vector & packet); +}; + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_SEYOND_DRIVER_H diff --git a/nebula_decoders/src/nebula_decoders_seyond/seyond_driver.cpp b/nebula_decoders/src/nebula_decoders_seyond/seyond_driver.cpp new file mode 100755 index 000000000..42ecabde2 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_seyond/seyond_driver.cpp @@ -0,0 +1,79 @@ +#include "nebula_decoders/nebula_decoders_seyond/seyond_driver.hpp" + +#include "nebula_decoders/nebula_decoders_seyond/decoders/falcon.hpp" +#include "nebula_decoders/nebula_decoders_seyond/decoders/robin_w.hpp" + +// #define WITH_DEBUG_STD_COUT_SEYOND_CLIENT // Use std::cout messages for debugging + +namespace nebula +{ +namespace drivers +{ +SeyondDriver::SeyondDriver( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & calibration_data) +{ + // initialize proper parser from cloud config's model and echo mode + driver_status_ = nebula::Status::OK; + + switch (sensor_configuration->sensor_model) { + case SensorModel::SEYOND_FALCON_KINETIC: + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + break; + case SensorModel::SEYOND_ROBIN_W: + scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + break; + case SensorModel::UNKNOWN: + driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; + throw std::runtime_error("Invalid sensor model."); + default: + driver_status_ = nebula::Status::NOT_INITIALIZED; + throw std::runtime_error("Driver not Implemented for selected sensor."); + } +} + +template +std::shared_ptr SeyondDriver::InitializeDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) +{ + // using CalibT = typename SensorT::angle_corrector_t::correction_data_t; + // return std::make_shared>( + // sensor_configuration, std::dynamic_pointer_cast(calibration_configuration)); +} + +std::tuple SeyondDriver::ParseCloudPacket( + const std::vector & packet) +{ + std::tuple pointcloud; + auto logger = rclcpp::get_logger("SeyondDriver"); + + if (driver_status_ != nebula::Status::OK) { + RCLCPP_ERROR(logger, "Driver not OK."); + return pointcloud; + } + + scan_decoder_->unpack(packet); + if (scan_decoder_->hasScanned()) { + pointcloud = scan_decoder_->getPointcloud(); + } + + return pointcloud; +} + +Status SeyondDriver::SetCalibrationConfiguration( + const SeyondCalibrationConfigurationBase & calibration_configuration) +{ + throw std::runtime_error( + "SetCalibrationConfiguration. Not yet implemented (" + + calibration_configuration.calibration_file + ")"); +} + +Status SeyondDriver::GetStatus() +{ + return driver_status_; +} + +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 32893d1f4..cf1ecffd4 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -26,6 +26,10 @@ include_directories( ${PCL_COMMON_INCLUDE_DIRS} ) +ament_auto_add_library(nebula_hw_interfaces_seyond SHARED + src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp + ) + ament_auto_add_library(nebula_hw_interfaces_hesai SHARED src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp ) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp new file mode 100755 index 000000000..f8d604559 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp @@ -0,0 +1,116 @@ +#ifndef NEBULA_SEYOND_HW_INTERFACE_H +#define NEBULA_SEYOND_HW_INTERFACE_H +// 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 +#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 +#define BOOST_BIND_GLOBAL_PLACEHOLDERS +#endif +#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 +#define BOOST_ALLOW_DEPRECATED_HEADERS +#endif +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" +#include "nebula_common/seyond/seyond_common.hpp" +#include "nebula_common/util/expected.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +/// @brief Hardware interface of seyond driver +class SeyondHwInterface +{ +private: + std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; + 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)> + cloud_packet_callback_; /**This function pointer is called when the scan is complete*/ + + int prev_phase_{}; + int target_model_no; + + std::shared_ptr parent_node_logger; + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + +public: + /// @brief Constructor + SeyondHwInterface(); + + /// @brief Destructor + ~SeyondHwInterface(); + + /// @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 ReceiveSensorPacketCallback(std::vector & buffer); + + /// @brief Starting the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStart(); + + /// @brief Function for stopping the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStop(); + + /// @brief Printing sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration); + + /// @brief Printing calibration configuration + /// @param calibration_configuration CalibrationConfiguration for the checking + /// @return Resulting status + Status GetCalibrationConfiguration(CalibrationConfigurationBase & calibration_configuration); + + /// @brief Setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration( + std::shared_ptr sensor_configuration); + + /// @brief Set target model number + /// @param model Model number + void SetTargetModel(int model); + + /// @brief Registering callback for NebulaPackets + /// @param scan_callback Callback function + /// @return Resulting status + Status RegisterScanCallback( + std::function &)> scan_callback); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr logger); + +}; +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_SEYOND_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp new file mode 100755 index 000000000..06eef22e0 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_seyond_hw_interfaces/seyond_hw_interface.cpp @@ -0,0 +1,90 @@ +#include "nebula_hw_interfaces/nebula_hw_interfaces_seyond/seyond_hw_interface.hpp" +namespace nebula +{ +namespace drivers +{ +SeyondHwInterface::SeyondHwInterface() +: cloud_io_context_{new ::drivers::common::IoContext(1)}, + info_io_context_{new ::drivers::common::IoContext(1)}, + cloud_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, + info_udp_driver_{new ::drivers::udp_driver::UdpDriver(*info_io_context_)}, + scan_cloud_ptr_{std::make_unique()} +{ +} + +void SeyondHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +{ + +} + +Status SeyondHwInterface::SensorInterfaceStart() +{ + return Status::OK; +} + +Status SeyondHwInterface::InfoInterfaceStart() +{ + return Status::OK; +} + +Status SeyondHwInterface::SensorInterfaceStop() +{ + return Status::ERROR_1; +} + +Status SeyondHwInterface::SetSensorConfiguration( + std::shared_ptr sensor_configuration) +{ + return Status::OK; +} + +Status SeyondHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +{ + return Status::ERROR_1; +} + +Status SeyondHwInterface::SetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) +{ + return Status::ERROR_1; +} + +Status SeyondHwInterface::GetCalibrationConfiguration( + CalibrationConfigurationBase & calibration_configuration) +{ + return Status::ERROR_1; +} + +Status SeyondHwInterface::RegisterScanCallback( + std::function scan_callback) +{ + return Status::OK; +} + +void SeyondHwInterface::SetTargetModel(int model) +{ + target_model_no = model; +} + +void SeyondHwInterface::PrintError(std::string error) +{ + +} + +void SeyondHwInterface::PrintDebug(std::string debug) +{ + +} + +void SeyondHwInterface::PrintInfo(std::string info) +{ + +} + +void SeyondHwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger_ = logger; +} + +} // namespace drivers +} // namespace nebula diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index f6bc6e987..14d42572c 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -35,18 +35,18 @@ include_directories( ) link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES}) -## Tutorial -ament_auto_add_library(tutorial_ros_wrapper SHARED - src/tutorial/tutorial_ros_wrapper.cpp - src/tutorial/decoder_wrapper.cpp - src/tutorial/hw_interface_wrapper.cpp - src/tutorial/hw_monitor_wrapper.cpp +## Seyond +ament_auto_add_library(seyond_ros_wrapper SHARED + src/seyond/seyond_ros_wrapper.cpp + src/seyond/decoder_wrapper.cpp + src/seyond/hw_interface_wrapper.cpp + src/seyond/hw_monitor_wrapper.cpp src/common/parameter_descriptors.cpp ) -rclcpp_components_register_node(tutorial_ros_wrapper - PLUGIN "TutorialRosWrapper" - EXECUTABLE tutorial_ros_wrapper_node +rclcpp_components_register_node(seyond_ros_wrapper + PLUGIN "SeyondRosWrapper" + EXECUTABLE seyond_ros_wrapper_node ) ## Hesai diff --git a/nebula_ros/include/nebula_ros/seyond/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/decoder_wrapper.hpp new file mode 100755 index 000000000..d6eaf61b2 --- /dev/null +++ b/nebula_ros/include/nebula_ros/seyond/decoder_wrapper.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +using SeyondDriver = nebula::drivers::SeyondDriver; +using SeyondHwInterface = nebula::drivers::SeyondHwInterface; +using SeyondSensorConfiguration = nebula::drivers::SeyondSensorConfiguration; +using SeyondCalibrationConfiguration = nebula::drivers::SeyondCalibrationConfiguration; +using get_calibration_result_t = + nebula::util::expected, nebula::Status>; + +class SeyondDecoderWrapper +{ +public: + SeyondDecoderWrapper( + rclcpp::Node * const parent_node, const std::shared_ptr & hw_interface, + std::shared_ptr & config); + + void ProcessCloudPacket(std::unique_ptr packet_msg); + + void OnConfigChange(const std::shared_ptr & new_config); + + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + nebula::Status Status(); + +private: + + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + + nebula::Status status_; + rclcpp::Logger logger_; + + const std::shared_ptr hw_interface_; + std::shared_ptr sensor_cfg_; + + std::string calibration_file_path_{}; + std::shared_ptr calibration_cfg_ptr_{}; + + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_{}; + + rclcpp::Publisher::SharedPtr nebula_points_pub_{}; + + std::shared_ptr cloud_watchdog_; +}; +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp new file mode 100755 index 000000000..6711ec0d4 --- /dev/null +++ b/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +using SeyondHwInterface = nebula::drivers::SeyondHwInterface; +using SeyondSensorConfiguration = nebula::drivers::SeyondSensorConfiguration; + +class SeyondHwInterfaceWrapper +{ +public: + SeyondHwInterfaceWrapper( + rclcpp::Node * const parent_node, std::shared_ptr & config); + + void OnConfigChange(const std::shared_ptr & new_config); + + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + std::shared_ptr hw_interface_; + rclcpp::Logger logger_; + nebula::Status status_; + bool setup_sensor_; +}; +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/seyond/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/hw_monitor_wrapper.hpp new file mode 100755 index 000000000..1b8ec6122 --- /dev/null +++ b/nebula_ros/include/nebula_ros/seyond/hw_monitor_wrapper.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +using SeyondHwInterface = nebula::drivers::SeyondHwInterface; +using SeyondSensorConfiguration = nebula::drivers::SeyondSensorConfiguration; + +class SeyondHwMonitorWrapper +{ +public: + SeyondHwMonitorWrapper(rclcpp::Node* const parent_node, + const std::shared_ptr& hw_interface, + std::shared_ptr& config); + + void OnConfigChange(const std::shared_ptr & /* new_config */) {} + + nebula::Status Status(); + +private: + void InitializeDiagnostics(); + + /// @brief Parse the stored diagnostic info and add it to the diagnostic status + void ParseAndAddDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + /// @brief Check if stored diagnostic info is up-to-date and force diagnostic updater update + void TriggerDiagnosticsUpdate(); + + /// @brief Get and store diagnostic info from sensor + void FetchDiagnosticInfo(); + + rclcpp::Node* const parent_node_; + rclcpp::Logger logger_; + nebula::Status status_; + uint16_t diag_span_; + diagnostic_updater::Updater diagnostics_updater_; + + const std::shared_ptr hw_interface_; + + rclcpp::TimerBase::SharedPtr diagnostics_update_timer_{}; + rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_{}; + + std::optional current_diag_info_{}; + std::optional current_diag_info_time_{}; + std::mutex mtx_current_diag_info_; +}; +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp new file mode 100755 index 000000000..d0697bde3 --- /dev/null +++ b/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/seyond/decoder_wrapper.hpp" +#include "nebula_ros/seyond/hw_interface_wrapper.hpp" +#include "nebula_ros/seyond/hw_monitor_wrapper.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +using SeyondHwInterface = nebula::drivers::SeyondHwInterface; +using SeyondSensorConfiguration = nebula::drivers::SeyondSensorConfiguration; +using SeyondCalibrationConfiguration = nebula::drivers::SeyondCalibrationConfiguration; + +class SeyondRosWrapper final : public rclcpp::Node +{ + +public: + explicit SeyondRosWrapper(const rclcpp::NodeOptions & options); + ~SeyondRosWrapper() noexcept {}; + + Status GetStatus(); + + Status StreamStart(); + +private: + void ReceiveCloudPacketCallback(std::vector & packet); + + void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + + Status DeclareAndGetSensorConfigParams(); + + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & new_config); + + Status wrapper_status_; + + std::shared_ptr sensor_cfg_ptr_{}; + std::mutex mtx_config_; + + mt_queue> packet_queue_; + + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_; + + std::optional hw_interface_wrapper_; + std::optional hw_monitor_wrapper_; + std::optional decoder_wrapper_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/launch/seyond.launch.xml b/nebula_ros/launch/seyond.launch.xml new file mode 100755 index 000000000..ceea8779d --- /dev/null +++ b/nebula_ros/launch/seyond.launch.xml @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/src/seyond/decoder_wrapper.cpp b/nebula_ros/src/seyond/decoder_wrapper.cpp new file mode 100755 index 000000000..0482d9eb7 --- /dev/null +++ b/nebula_ros/src/seyond/decoder_wrapper.cpp @@ -0,0 +1,149 @@ +#include "nebula_ros/seyond/decoder_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +using namespace std::chrono_literals; + +SeyondDecoderWrapper::SeyondDecoderWrapper(rclcpp::Node* const parent_node, + const std::shared_ptr& hw_interface, + std::shared_ptr& config) + : status_(nebula::Status::NOT_INITIALIZED) + , logger_(parent_node->get_logger().get_child("SeyondDecoder")) + , hw_interface_(hw_interface) + , sensor_cfg_(config) +{ + if (!config) + { + throw std::runtime_error("SeyondDecoderWrapper cannot be instantiated without a valid config!"); + } + + RCLCPP_INFO(logger_, "Starting Decoder"); + + driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) + { + throw std::runtime_error((std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_) + { + current_scan_msg_ = std::make_unique(); + packets_pub_ = + parent_node->create_publisher("nebula_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + nebula_points_pub_ = parent_node->create_publisher("nebula_points", pointcloud_qos); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) + return; + RCLCPP_WARN_THROTTLE(logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); +} + +void SeyondDecoderWrapper::OnConfigChange( + const std::shared_ptr& new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); + driver_ptr_ = new_driver; + sensor_cfg_ = new_config; +} + +void SeyondDecoderWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) +{ + // //////////////////////////////////////// + // Accumulate packets for recording + // //////////////////////////////////////// + + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) + if (hw_interface_ && + (packets_pub_->get_subscription_count() > 0 || packets_pub_->get_intra_process_subscription_count() > 0)) + { + if (current_scan_msg_->packets.size() == 0) + { + current_scan_msg_->header.stamp = packet_msg->stamp; + } + + current_scan_msg_->packets.emplace_back(*packet_msg); + } + + // //////////////////////////////////////// + // Decode packet + // //////////////////////////////////////// + + std::tuple pointcloud_ts{}; + nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; + { + std::lock_guard lock(mtx_driver_ptr_); + pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud = std::get<0>(pointcloud_ts); + } + + if (pointcloud == nullptr) + { + return; + }; + + // //////////////////////////////////////// + // If scan completed, publish pointcloud + // //////////////////////////////////////// + + // A pointcloud has been produced, reset the watchdog timer + cloud_watchdog_->update(); + + // Publish scan message only if it has been written to + if (current_scan_msg_ && !current_scan_msg_->packets.empty()) + { + packets_pub_->publish(std::move(current_scan_msg_)); + current_scan_msg_ = std::make_unique(); + } + + if (nebula_points_pub_->get_subscription_count() > 0 || + nebula_points_pub_->get_intra_process_subscription_count() > 0) + { + auto ros_pc_msg_ptr = std::make_unique(); + pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); + auto nanoseconds = std::chrono::duration_cast( + std::chrono::duration(std::get<1>(pointcloud_ts))) + .count(); + ros_pc_msg_ptr->header.stamp = rclcpp::Time(nanoseconds); + PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + } +} + +void SeyondDecoderWrapper::PublishCloud(std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr& publisher) +{ + if (pointcloud->header.stamp.sec < 0) + { + RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +nebula::Status SeyondDecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) + { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/seyond/hw_interface_wrapper.cpp b/nebula_ros/src/seyond/hw_interface_wrapper.cpp new file mode 100755 index 000000000..ed9092784 --- /dev/null +++ b/nebula_ros/src/seyond/hw_interface_wrapper.cpp @@ -0,0 +1,65 @@ +#include "nebula_ros/seyond/hw_interface_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ + +SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper( + rclcpp::Node* const parent_node, std::shared_ptr& config) + : hw_interface_(new SeyondHwInterface()) + , logger_(parent_node->get_logger().get_child("HwInterface")) + , status_(Status::NOT_INITIALIZED) +{ + setup_sensor_ = parent_node->declare_parameter("setup_sensor", true, param_read_only()); + bool retry_connect = parent_node->declare_parameter("retry_hw", true, param_read_only()); + + status_ = hw_interface_->SetSensorConfiguration(config); + + if (status_ != Status::OK) { + throw std::runtime_error((std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + hw_interface_->SetLogger(std::make_shared(parent_node->get_logger())); + //hw_interface_->SetTargetModel(config->sensor_model); + + int retry_count = 0; + + // NOTE: for when TP interface is implemented + // while (true) + // { + // status_ = hw_interface_->InitializeTcpDriver(); + // if (status_ == Status::OK || !retry_connect) + // { + // break; + // } + + // retry_count++; + // std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + // RCLCPP_WARN_STREAM(logger_, status_ << ". Retry #" << retry_count); + // } + + status_ = Status::OK; +} + +void SeyondHwInterfaceWrapper::OnConfigChange( + const std::shared_ptr& new_config) +{ + hw_interface_->SetSensorConfiguration(new_config); + // if (setup_sensor_) { + // hw_interface_->CheckAndSetConfig(); + // } +} + +Status SeyondHwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr SeyondHwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/seyond/hw_monitor_wrapper.cpp b/nebula_ros/src/seyond/hw_monitor_wrapper.cpp new file mode 100755 index 000000000..2ace93a03 --- /dev/null +++ b/nebula_ros/src/seyond/hw_monitor_wrapper.cpp @@ -0,0 +1,82 @@ +#include "nebula_ros/seyond/hw_monitor_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +SeyondHwMonitorWrapper::SeyondHwMonitorWrapper(rclcpp::Node* const parent_node, + const std::shared_ptr& hw_interface, + std::shared_ptr& /* config */) + : parent_node_(parent_node) + , logger_(parent_node->get_logger().get_child("HwMonitor")) + , status_(Status::OK) + , diagnostics_updater_(parent_node) + , hw_interface_(hw_interface) +{ + diag_span_ = parent_node->declare_parameter("diag_span", 1000, param_read_only()); + + InitializeDiagnostics(); +} + +void SeyondHwMonitorWrapper::InitializeDiagnostics() +{ + std::string hardware_id = ""; // get hardware ID (model:serial_number) from sensor here + diagnostics_updater_.setHardwareID(hardware_id); + RCLCPP_INFO_STREAM(logger_, "hardware_id: " + hardware_id); + + diagnostics_updater_.add("diagnostic_info", this, &SeyondHwMonitorWrapper::ParseAndAddDiagnosticInfo); + + // Timer to fetch new info from sensor. Choose a sensible period, e.g. once per sec. + fetch_diagnostics_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(1000), + std::bind(&SeyondHwMonitorWrapper::FetchDiagnosticInfo, this)); + + // Timer to trigger diagnostic_updater updates and to check if fetched data is stale + diagnostics_update_timer_ = + parent_node_->create_wall_timer(std::chrono::milliseconds(diag_span_), + std::bind(&SeyondHwMonitorWrapper::TriggerDiagnosticsUpdate, this)); +} + +void SeyondHwMonitorWrapper::ParseAndAddDiagnosticInfo(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + diagnostics.add("voltage", current_diag_info_->get("status.voltage")); + diagnostics.add("error", current_diag_info_->get("status.error")); + // etc. + + diagnostics.summary(level, "OK"); +} + +void SeyondHwMonitorWrapper::TriggerDiagnosticsUpdate() +{ + RCLCPP_DEBUG_STREAM(logger_, "OnUpdateTimer"); + auto now = parent_node_->get_clock()->now(); + auto dif = (now - *current_diag_info_time_).seconds(); + + RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); + + if (diag_span_ * 2.0 < dif * 1000) + { + RCLCPP_DEBUG_STREAM(logger_, "STALE"); + } + else + { + RCLCPP_DEBUG_STREAM(logger_, "OK"); + } + + diagnostics_updater_.force_update(); +} + +void SeyondHwMonitorWrapper::FetchDiagnosticInfo() +{ + current_diag_info_ = {}; // fetch from sensor here + current_diag_info_time_ = parent_node_->now(); +} + +Status SeyondHwMonitorWrapper::Status() +{ + return Status::NOT_IMPLEMENTED; // TODO +} +} // namespace ros +} // namespace nebula \ No newline at end of file diff --git a/nebula_ros/src/seyond/seyond_ros_wrapper.cpp b/nebula_ros/src/seyond/seyond_ros_wrapper.cpp new file mode 100755 index 000000000..8a94cbb1b --- /dev/null +++ b/nebula_ros/src/seyond/seyond_ros_wrapper.cpp @@ -0,0 +1,334 @@ +#include "nebula_ros/seyond/seyond_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +SeyondRosWrapper::SeyondRosWrapper(const rclcpp::NodeOptions& options) + : rclcpp::Node("seyond_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_() +{ + // //////////////////////////////////////// + // Define, get and validate ROS parameters + // //////////////////////////////////////// + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) + { + throw std::runtime_error((std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig: " << *sensor_cfg_ptr_); + + // //////////////////////////////////////// + // Launch hardware wrappers (if enabled) + // //////////////////////////////////////// + + 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, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); + } + + // //////////////////////////////////////// + // Get calibration data and launch decoder + // //////////////////////////////////////// + + decoder_wrapper_.emplace( + this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + // The decoder is running in its own thread to not block UDP reception + decoder_thread_ = std::thread([this]() { + while (true) + { + decoder_wrapper_->ProcessCloudPacket(std::move(packet_queue_.pop())); + } + }); + + // //////////////////////////////////////// + // Configure packet / scan message routing + // //////////////////////////////////////// + + if (launch_hw_) + { + hw_interface_wrapper_->HwInterface()->RegisterScanCallback( + std::bind(&SeyondRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + StreamStart(); + } + else + { + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind(&SeyondRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM(get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // //////////////////////////////////////// + // Enable callbacks for config changes + // //////////////////////////////////////// + + // Register parameter callback after all params have been declared. Otherwise it would be called once for each + // declaration + parameter_event_cb_ = + add_on_set_parameters_callback(std::bind(&SeyondRosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status SeyondRosWrapper::DeclareAndGetSensorConfigParams() +{ + SeyondSensorConfiguration config; + + auto _sensor_model = declare_parameter("sensor_model", "", param_read_only()); + config.sensor_model = drivers::SensorModelFromString(_sensor_model); + + auto _return_mode = declare_parameter("return_mode", "", param_read_write()); + config.return_mode = drivers::ReturnModeFromString(_return_mode); + + config.host_ip = declare_parameter("host_ip", "255.255.255.255", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", "192.168.1.201", param_read_only()); + config.data_port = declare_parameter("data_port", 2368, param_read_only()); + config.gnss_port = declare_parameter("gnss_port", 2369, param_read_only()); + config.frame_id = declare_parameter("frame_id", "seyond", param_read_write()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Angle where scans begin (degrees, [0.,360.])"; + descriptor.floating_point_range = float_range(0, 360, 0.01); + config.scan_phase = declare_parameter("scan_phase", 0., descriptor); + } + + config.min_range = declare_parameter("min_range", 0.3, param_read_write()); + config.max_range = declare_parameter("max_range", 300., param_read_write()); + config.packet_mtu_size = declare_parameter("packet_mtu_size", 1500, param_read_only()); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "300, 600, 1200"; + descriptor.integer_range = int_range(300, 1200, 300); + config.rotation_speed = declare_parameter("rotation_speed", 600, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_min_angle = declare_parameter("cloud_min_angle", 0, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.integer_range = int_range(0, 360, 1); + config.cloud_max_angle = declare_parameter("cloud_max_angle", 360, descriptor); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.additional_constraints = "Dual return distance threshold [0.01, 0.5]"; + descriptor.floating_point_range = float_range(0.01, 0.5, 0.01); + config.dual_return_distance_threshold = + declare_parameter("dual_return_distance_threshold", 0.1, descriptor); + } + + auto _ptp_profile = declare_parameter("ptp_profile", "", param_read_only()); + config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); + + auto _ptp_transport = declare_parameter("ptp_transport_type", "", param_read_only()); + config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); + + auto _ptp_switch = declare_parameter("ptp_switch_type", "", param_read_only()); + config.ptp_switch_type = drivers::PtpSwitchTypeFromString(_ptp_switch); + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.integer_range = int_range(0, 127, 1); + config.ptp_domain = declare_parameter("ptp_domain", 0, descriptor); + } + + auto new_cfg_ptr = std::make_shared(config); + return ValidateAndSetConfig(new_cfg_ptr); +} + +Status SeyondRosWrapper::ValidateAndSetConfig(std::shared_ptr& new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) + { + return Status::INVALID_SENSOR_MODEL; + } + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) + { + return Status::INVALID_ECHO_MODE; + } + if (new_config->frame_id.empty()) + { + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_profile == nebula::drivers::PtpProfile::UNKNOWN_PROFILE) + { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_transport_type == nebula::drivers::PtpTransportType::UNKNOWN_TRANSPORT) + { + RCLCPP_ERROR_STREAM(get_logger(), + "Invalid PTP Transport Provided. Please use 'udp' or 'l2', 'udp' is only available when " + "using the '1588v2' PTP Profile"); + return Status::SENSOR_CONFIG_ERROR; + } + if (new_config->ptp_switch_type == nebula::drivers::PtpSwitchType::UNKNOWN_SWITCH) + { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Switch Type Provided. Please use 'tsn' or 'non_tsn'"); + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) + { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (hw_monitor_wrapper_) + { + hw_monitor_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) + { + decoder_wrapper_->OnConfigChange(new_config); + } + + sensor_cfg_ptr_ = new_config; + return Status::OK; +} + +void SeyondRosWrapper::ReceiveScanMessageCallback(std::unique_ptr scan_msg) +{ + if (hw_interface_wrapper_) + { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, + "Ignoring received PandarScan. Launch with launch_hw:=false to enable PandarScan replay."); + return; + } + + for (auto& pkt : scan_msg->packets) + { + auto nebula_pkt_ptr = std::make_unique(); + nebula_pkt_ptr->stamp = pkt.stamp; + std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); + + packet_queue_.push(std::move(nebula_pkt_ptr)); + } +} + +Status SeyondRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status SeyondRosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) + { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) + { + return hw_interface_wrapper_->Status(); + } + + return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); +} + +rcl_interfaces::msg::SetParametersResult SeyondRosWrapper::OnParameterChange(const std::vector& p) +{ + std::lock_guard lock(mtx_config_); + using namespace rcl_interfaces::msg; + + if (p.empty()) + { + return rcl_interfaces::build().successful(true).reason(""); + } + + // //////////////////////////////////////// + // Update sub-wrapper parameters (if any) + // //////////////////////////////////////// + + // Currently, HW interface and monitor wrappers have only read-only parameters, so their update logic is not + // implemented + if (decoder_wrapper_) + { + auto result = decoder_wrapper_->OnParameterChange(p); + if (!result.successful) { + return result; + } + } + + // //////////////////////////////////////// + // Create new, updated sensor configuration + // //////////////////////////////////////// + + SeyondSensorConfiguration new_cfg(*sensor_cfg_ptr_); + + std::string _return_mode = ""; + bool got_any = get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | + get_param(p, "scan_phase", new_cfg.scan_phase) | get_param(p, "min_range", new_cfg.min_range) | + get_param(p, "max_range", new_cfg.max_range) | get_param(p, "rotation_speed", new_cfg.rotation_speed) | + get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | + get_param(p, "dual_return_distance_threshold", new_cfg.dual_return_distance_threshold); + + if (!got_any) + { + return rcl_interfaces::build().successful(true).reason(""); + } + + if (_return_mode.length() > 0) + new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + + // //////////////////////////////////////// + // Validate and use new config + // //////////////////////////////////////// + + auto new_cfg_ptr = std::make_shared(new_cfg); + auto status = ValidateAndSetConfig(new_cfg_ptr); + + if (status != Status::OK) + { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void SeyondRosWrapper::ReceiveCloudPacketCallback(std::vector& packet) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) + { + return; + } + + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); + + 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); + + // If the decoder is too slow (= the queue becomes full), packets are dropped here + if (!packet_queue_.try_push(std::move(msg_ptr))) + { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(SeyondRosWrapper) +} // namespace ros +} // namespace nebula