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 index 91099dd46..c3570e4ea 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/falcon.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_seyond/decoders/falcon.hpp @@ -15,7 +15,6 @@ namespace seyond_packet struct PacketFalcon : public PacketBase<8, 32, 2, 100> { - }; #pragma pack(pop) @@ -24,9 +23,7 @@ struct PacketFalcon : public PacketBase<8, 32, 2, 100> class Falcon : public SeyondSensor { - - }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula 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 index 7188088c5..831ebacbf 100644 --- 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 @@ -15,7 +15,6 @@ namespace seyond_packet struct PacketRobinW : public PacketBase<8, 32, 2, 100> { - }; #pragma pack(pop) @@ -24,9 +23,7 @@ struct PacketRobinW : public PacketBase<8, 32, 2, 100> class RobinW : public SeyondSensor { - - }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula 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 old mode 100755 new mode 100644 index 34d5039e8..ea337fc18 --- 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 @@ -34,7 +34,7 @@ class SeyondDecoder : public SeyondScanDecoder /// @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 + 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 @@ -78,7 +78,8 @@ class SeyondDecoder : public SeyondScanDecoder /// @param correction_data Calibration data for this decoder explicit SeyondDecoder( const std::shared_ptr & sensor_configuration, - const std::shared_ptr & correction_data) + const std::shared_ptr & + correction_data) : sensor_configuration_(sensor_configuration), angle_corrector_(correction_data), logger_(rclcpp::get_logger("SeyondDecoder")) @@ -93,10 +94,7 @@ class SeyondDecoder : public SeyondScanDecoder output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); } - int unpack(const std::vector & packet) override - { - return -1; - } + int unpack(const std::vector & packet) override { return -1; } bool hasScanned() override { return has_scanned_; } 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 old mode 100755 new mode 100644 index 8f647c109..610137acd --- 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 @@ -1,8 +1,8 @@ #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_common/seyond/seyond_common.hpp" #include "nebula_msgs/msg/nebula_packet.hpp" #include "nebula_msgs/msg/nebula_packets.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 index 88c0a4125..2bf7c43c1 100644 --- 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 @@ -16,7 +16,6 @@ template class SeyondSensor { private: - public: typedef PacketT packet_t; @@ -25,4 +24,4 @@ class SeyondSensor }; } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula 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 old mode 100755 new mode 100644 index 5e028760c..7b11b4a26 --- 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 @@ -86,7 +86,8 @@ class SeyondHwInterface /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status - Status GetCalibrationConfiguration(const CalibrationConfigurationBase & calibration_configuration); + Status GetCalibrationConfiguration( + const CalibrationConfigurationBase & calibration_configuration); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface @@ -101,13 +102,11 @@ class SeyondHwInterface /// @brief Registering callback for NebulaPackets /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function &)> scan_callback); + Status RegisterScanCallback(std::function &)> scan_callback); /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr logger); - }; } // namespace drivers } // namespace nebula 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 old mode 100755 new mode 100644 index ffe3f89e1..bd9737203 --- 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 @@ -12,7 +12,6 @@ SeyondHwInterface::SeyondHwInterface() void SeyondHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { - } Status SeyondHwInterface::SensorInterfaceStart() @@ -31,7 +30,8 @@ Status SeyondHwInterface::SetSensorConfiguration( return Status::OK; } -Status SeyondHwInterface::GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration) +Status SeyondHwInterface::GetSensorConfiguration( + const SensorConfigurationBase & sensor_configuration) { return Status::ERROR_1; } @@ -55,17 +55,14 @@ void SeyondHwInterface::SetTargetModel(int 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) diff --git a/nebula_ros/include/nebula_ros/seyond/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/decoder_wrapper.hpp old mode 100755 new mode 100644 index d6eaf61b2..27519eae0 --- a/nebula_ros/include/nebula_ros/seyond/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/seyond/decoder_wrapper.hpp @@ -44,7 +44,6 @@ class SeyondDecoderWrapper nebula::Status Status(); private: - void PublishCloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher); @@ -69,4 +68,4 @@ class SeyondDecoderWrapper std::shared_ptr cloud_watchdog_; }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp index 6711ec0d4..bf5a976f4 100755 --- a/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/seyond/hw_interface_wrapper.hpp @@ -34,4 +34,4 @@ class SeyondHwInterfaceWrapper bool setup_sensor_; }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/seyond/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/hw_monitor_wrapper.hpp old mode 100755 new mode 100644 index 1b8ec6122..3e78931d1 --- a/nebula_ros/include/nebula_ros/seyond/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/seyond/hw_monitor_wrapper.hpp @@ -4,7 +4,6 @@ #include #include - #include #include @@ -26,9 +25,9 @@ using SeyondSensorConfiguration = nebula::drivers::SeyondSensorConfiguration; class SeyondHwMonitorWrapper { public: - SeyondHwMonitorWrapper(rclcpp::Node* const parent_node, - const std::shared_ptr& hw_interface, - std::shared_ptr& config); + SeyondHwMonitorWrapper( + rclcpp::Node * const parent_node, const std::shared_ptr & hw_interface, + std::shared_ptr & config); void OnConfigChange(const std::shared_ptr & /* new_config */) {} @@ -46,7 +45,7 @@ class SeyondHwMonitorWrapper /// @brief Get and store diagnostic info from sensor void FetchDiagnosticInfo(); - rclcpp::Node* const parent_node_; + rclcpp::Node * const parent_node_; rclcpp::Logger logger_; nebula::Status status_; uint16_t diag_span_; @@ -62,4 +61,4 @@ class SeyondHwMonitorWrapper std::mutex mtx_current_diag_info_; }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp old mode 100755 new mode 100644 index d0697bde3..b04e72083 --- a/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/seyond/seyond_ros_wrapper.hpp @@ -37,7 +37,6 @@ using SeyondCalibrationConfiguration = nebula::drivers::SeyondCalibrationConfigu class SeyondRosWrapper final : public rclcpp::Node { - public: explicit SeyondRosWrapper(const rclcpp::NodeOptions & options); ~SeyondRosWrapper() noexcept {}; @@ -56,8 +55,7 @@ class SeyondRosWrapper final : public rclcpp::Node rcl_interfaces::msg::SetParametersResult OnParameterChange( const std::vector & p); - Status ValidateAndSetConfig( - std::shared_ptr & new_config); + Status ValidateAndSetConfig(std::shared_ptr & new_config); Status wrapper_status_; diff --git a/nebula_ros/src/seyond/decoder_wrapper.cpp b/nebula_ros/src/seyond/decoder_wrapper.cpp old mode 100755 new mode 100644 index 0482d9eb7..8e1ceb77c --- a/nebula_ros/src/seyond/decoder_wrapper.cpp +++ b/nebula_ros/src/seyond/decoder_wrapper.cpp @@ -7,16 +7,15 @@ 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) +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) - { + if (!config) { throw std::runtime_error("SeyondDecoderWrapper cannot be instantiated without a valid config!"); } @@ -25,35 +24,37 @@ SeyondDecoderWrapper::SeyondDecoderWrapper(rclcpp::Node* const parent_node, 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()); + 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_) - { + if (hw_interface_) { current_scan_msg_ = std::make_unique(); - packets_pub_ = - parent_node->create_publisher("nebula_packets", rclcpp::SensorDataQoS()); + 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); + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - nebula_points_pub_ = parent_node->create_publisher("nebula_points", pointcloud_qos); + 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"); - }); + 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) + const std::shared_ptr & new_config) { std::lock_guard lock(mtx_driver_ptr_); auto new_driver = std::make_shared(new_config, calibration_cfg_ptr_); @@ -61,18 +62,18 @@ void SeyondDecoderWrapper::OnConfigChange( sensor_cfg_ = new_config; } -void SeyondDecoderWrapper::ProcessCloudPacket(std::unique_ptr packet_msg) +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) - { + 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; } @@ -91,8 +92,7 @@ void SeyondDecoderWrapper::ProcessCloudPacket(std::unique_ptr(pointcloud_ts); } - if (pointcloud == nullptr) - { + if (pointcloud == nullptr) { return; }; @@ -104,15 +104,14 @@ void SeyondDecoderWrapper::ProcessCloudPacket(std::unique_ptrupdate(); // Publish scan message only if it has been written to - if (current_scan_msg_ && !current_scan_msg_->packets.empty()) - { + 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) - { + 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( @@ -123,11 +122,11 @@ void SeyondDecoderWrapper::ProcessCloudPacket(std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr& publisher) +void SeyondDecoderWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) { - if (pointcloud->header.stamp.sec < 0) - { + if (pointcloud->header.stamp.sec < 0) { RCLCPP_WARN_STREAM(logger_, "Timestamp error, verify clock source."); } pointcloud->header.frame_id = sensor_cfg_->frame_id; @@ -138,12 +137,11 @@ nebula::Status SeyondDecoderWrapper::Status() { std::lock_guard lock(mtx_driver_ptr_); - if (!driver_ptr_) - { + if (!driver_ptr_) { return nebula::Status::NOT_INITIALIZED; } return driver_ptr_->GetStatus(); } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/seyond/hw_interface_wrapper.cpp b/nebula_ros/src/seyond/hw_interface_wrapper.cpp old mode 100755 new mode 100644 index ed9092784..2223fcee0 --- a/nebula_ros/src/seyond/hw_interface_wrapper.cpp +++ b/nebula_ros/src/seyond/hw_interface_wrapper.cpp @@ -6,10 +6,10 @@ 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) + 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()); @@ -17,11 +17,12 @@ SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper( status_ = hw_interface_->SetSensorConfiguration(config); if (status_ != Status::OK) { - throw std::runtime_error((std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + 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); + // hw_interface_->SetTargetModel(config->sensor_model); int retry_count = 0; @@ -43,7 +44,7 @@ SeyondHwInterfaceWrapper::SeyondHwInterfaceWrapper( } void SeyondHwInterfaceWrapper::OnConfigChange( - const std::shared_ptr& new_config) + const std::shared_ptr & new_config) { hw_interface_->SetSensorConfiguration(new_config); // if (setup_sensor_) { @@ -62,4 +63,4 @@ std::shared_ptr SeyondHwInterfaceWrapper::HwInterface() const } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/seyond/hw_monitor_wrapper.cpp b/nebula_ros/src/seyond/hw_monitor_wrapper.cpp old mode 100755 new mode 100644 index 2ace93a03..75d659178 --- a/nebula_ros/src/seyond/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/seyond/hw_monitor_wrapper.cpp @@ -4,14 +4,14 @@ 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) +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()); @@ -20,24 +20,25 @@ SeyondHwMonitorWrapper::SeyondHwMonitorWrapper(rclcpp::Node* const parent_node, void SeyondHwMonitorWrapper::InitializeDiagnostics() { - std::string hardware_id = ""; // get hardware ID (model:serial_number) from sensor here + 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); + 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)); + 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)); + 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) +void SeyondHwMonitorWrapper::ParseAndAddDiagnosticInfo( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -56,12 +57,9 @@ void SeyondHwMonitorWrapper::TriggerDiagnosticsUpdate() RCLCPP_DEBUG_STREAM(logger_, "dif(status): " << dif); - if (diag_span_ * 2.0 < dif * 1000) - { + if (diag_span_ * 2.0 < dif * 1000) { RCLCPP_DEBUG_STREAM(logger_, "STALE"); - } - else - { + } else { RCLCPP_DEBUG_STREAM(logger_, "OK"); } @@ -70,7 +68,7 @@ void SeyondHwMonitorWrapper::TriggerDiagnosticsUpdate() void SeyondHwMonitorWrapper::FetchDiagnosticInfo() { - current_diag_info_ = {}; // fetch from sensor here + current_diag_info_ = {}; // fetch from sensor here current_diag_info_time_ = parent_node_->now(); } @@ -79,4 +77,4 @@ Status SeyondHwMonitorWrapper::Status() return Status::NOT_IMPLEMENTED; // TODO } } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/seyond/seyond_ros_wrapper.cpp b/nebula_ros/src/seyond/seyond_ros_wrapper.cpp old mode 100755 new mode 100644 index 8a94cbb1b..0eae442fb --- a/nebula_ros/src/seyond/seyond_ros_wrapper.cpp +++ b/nebula_ros/src/seyond/seyond_ros_wrapper.cpp @@ -4,24 +4,24 @@ 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_() +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()); + 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_); @@ -32,8 +32,7 @@ SeyondRosWrapper::SeyondRosWrapper(const rclcpp::NodeOptions& options) launch_hw_ = declare_parameter("launch_hw", param_read_only()); - if (launch_hw_) - { + if (launch_hw_) { hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); } @@ -49,8 +48,7 @@ SeyondRosWrapper::SeyondRosWrapper(const rclcpp::NodeOptions& options) // The decoder is running in its own thread to not block UDP reception decoder_thread_ = std::thread([this]() { - while (true) - { + while (true) { decoder_wrapper_->ProcessCloudPacket(std::move(packet_queue_.pop())); } }); @@ -59,29 +57,27 @@ SeyondRosWrapper::SeyondRosWrapper(const rclcpp::NodeOptions& options) // Configure packet / scan message routing // //////////////////////////////////////// - if (launch_hw_) - { + if (launch_hw_) { hw_interface_wrapper_->HwInterface()->RegisterScanCallback( - std::bind(&SeyondRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + std::bind(&SeyondRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); StreamStart(); - } - else - { + } 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()); + "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)); + // 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() @@ -95,7 +91,8 @@ nebula::Status SeyondRosWrapper::DeclareAndGetSensorConfigParams() 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.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()); @@ -132,7 +129,7 @@ nebula::Status SeyondRosWrapper::DeclareAndGetSensorConfigParams() 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); + declare_parameter("dual_return_distance_threshold", 0.1, descriptor); } auto _ptp_profile = declare_parameter("ptp_profile", "", param_read_only()); @@ -154,48 +151,43 @@ nebula::Status SeyondRosWrapper::DeclareAndGetSensorConfigParams() return ValidateAndSetConfig(new_cfg_ptr); } -Status SeyondRosWrapper::ValidateAndSetConfig(std::shared_ptr& new_config) +Status SeyondRosWrapper::ValidateAndSetConfig( + std::shared_ptr & new_config) { - if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) - { + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; } - if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) - { + if (new_config->return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if (new_config->frame_id.empty()) - { + 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'"); + 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"); + 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'"); + 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_) - { + if (hw_interface_wrapper_) { hw_interface_wrapper_->OnConfigChange(new_config); } - if (hw_monitor_wrapper_) - { + if (hw_monitor_wrapper_) { hw_monitor_wrapper_->OnConfigChange(new_config); } - if (decoder_wrapper_) - { + if (decoder_wrapper_) { decoder_wrapper_->OnConfigChange(new_config); } @@ -203,17 +195,17 @@ Status SeyondRosWrapper::ValidateAndSetConfig(std::shared_ptr scan_msg) +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."); + 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) - { + 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)); @@ -229,26 +221,24 @@ Status SeyondRosWrapper::GetStatus() Status SeyondRosWrapper::StreamStart() { - if (!hw_interface_wrapper_) - { + if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; } - if (hw_interface_wrapper_->Status() != Status::OK) - { + 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) +rcl_interfaces::msg::SetParametersResult SeyondRosWrapper::OnParameterChange( + const std::vector & p) { std::lock_guard lock(mtx_config_); using namespace rcl_interfaces::msg; - if (p.empty()) - { + if (p.empty()) { return rcl_interfaces::build().successful(true).reason(""); } @@ -256,10 +246,9 @@ rcl_interfaces::msg::SetParametersResult SeyondRosWrapper::OnParameterChange(con // 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_) - { + // 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; @@ -273,15 +262,16 @@ rcl_interfaces::msg::SetParametersResult SeyondRosWrapper::OnParameterChange(con 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) - { + 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(""); } @@ -295,8 +285,7 @@ rcl_interfaces::msg::SetParametersResult SeyondRosWrapper::OnParameterChange(con auto new_cfg_ptr = std::make_shared(new_cfg); auto status = ValidateAndSetConfig(new_cfg_ptr); - if (status != Status::OK) - { + if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); auto result = SetParametersResult(); result.successful = false; @@ -307,15 +296,15 @@ rcl_interfaces::msg::SetParametersResult SeyondRosWrapper::OnParameterChange(con return rcl_interfaces::build().successful(true).reason(""); } -void SeyondRosWrapper::ReceiveCloudPacketCallback(std::vector& packet) +void SeyondRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) { - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) - { + 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(); + 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); @@ -323,8 +312,7 @@ void SeyondRosWrapper::ReceiveCloudPacketCallback(std::vector& packet) 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))) - { + if (!packet_queue_.try_push(std::move(msg_ptr))) { RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); } }