diff --git a/nebula_messages/nebula_msgs/CMakeLists.txt b/nebula_messages/nebula_msgs/CMakeLists.txt new file mode 100644 index 000000000..1514b5342 --- /dev/null +++ b/nebula_messages/nebula_msgs/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(nebula_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/RawPacketStamped.msg" + DEPENDENCIES + builtin_interfaces + ) + +ament_package() diff --git a/nebula_messages/nebula_msgs/msg/RawPacketStamped.msg b/nebula_messages/nebula_msgs/msg/RawPacketStamped.msg new file mode 100644 index 000000000..f6ba285d8 --- /dev/null +++ b/nebula_messages/nebula_msgs/msg/RawPacketStamped.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +uint8[] data diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml new file mode 100644 index 000000000..a4e02849a --- /dev/null +++ b/nebula_messages/nebula_msgs/package.xml @@ -0,0 +1,23 @@ + + + nebula_msgs + 0.0.0 + + ROS message definitions for timestamped UDP packets. + + Max Schmeller + Apache 2 + + ament_cmake_auto + rosidl_default_generators + + builtin_interfaces + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 34a4033cc..1c116783d 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(nebula_decoders REQUIRED) find_package(nebula_hw_interfaces REQUIRED) find_package(yaml-cpp REQUIRED) find_package(robosense_msgs REQUIRED) +find_package(nebula_msgs REQUIRED) include_directories( include diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 4dbe5722c..9ffc44207 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -15,8 +15,7 @@ #include #include -#include "pandar_msgs/msg/pandar_packet.hpp" -#include "pandar_msgs/msg/pandar_scan.hpp" +#include "nebula_msgs/msg/raw_packet_stamped.hpp" #include #include @@ -72,23 +71,21 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase Status Shutdown() override; private: - /// @brief Initializing ros wrapper + /// @brief Initialize pointcloud decoder /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeCloudDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) override; - - /// @brief Initializing ros wrapper for AT - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @param correction_configuration CorrectionConfiguration for this driver + /// @param correction_configuration CorrectionConfiguration for this driver (only for AT128, ignored otherwise) /// @return Resulting status Status InitializeCloudDriver( const std::shared_ptr & sensor_configuration, const std::shared_ptr & calibration_configuration, - const std::shared_ptr & correction_configuration); + const std::shared_ptr & correction_configuration = nullptr); + + Status InitializeCloudDriver( + std::shared_ptr sensor_configuration, + std::shared_ptr calibration_configuration) { + return InitializeCloudDriver(sensor_configuration, calibration_configuration, nullptr); + } /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration @@ -188,7 +185,10 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase std::shared_ptr driver_ptr_; Status wrapper_status_; - rclcpp::Subscription::SharedPtr pandar_scan_sub_; + + rclcpp::Publisher::SharedPtr packet_pub_; + rclcpp::Subscription::SharedPtr packet_sub_; + rclcpp::Publisher::SharedPtr nebula_points_pub_; rclcpp::Publisher::SharedPtr aw_points_ex_pub_; rclcpp::Publisher::SharedPtr aw_points_base_pub_; diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml index 71302b746..715a63a1a 100644 --- a/nebula_ros/launch/hesai_launch_all_hw.xml +++ b/nebula_ros/launch/hesai_launch_all_hw.xml @@ -27,8 +27,6 @@ - - @@ -55,7 +53,6 @@ - @@ -63,6 +60,5 @@ - diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index c9c86d896..cba7b51c6 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -24,6 +24,7 @@ yaml-cpp velodyne_msgs robosense_msgs + nebula_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 5aab808a7..a83f6a584 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -5,7 +5,9 @@ namespace nebula namespace ros { HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) +: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + hw_interface_(), + diagnostics_updater_(this) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -14,33 +16,29 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) sensor_cfg_ptr_ = std::make_shared(sensor_configuration); // hwiface -{ - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_hw_ms_)); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr_)); -#if not defined(TEST_PCAP) - Status rt = hw_interface_.InitializeTcpDriver(); - if(this->retry_hw_) { - int cnt = 0; - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { - cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - rt = hw_interface_.InitializeTcpDriver(); + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr_)); + Status rt = hw_interface_.InitializeTcpDriver(); + if (this->retry_hw_) { + int cnt = 0; + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); + while (rt == Status::ERROR_1) { + cnt++; + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); + rt = hw_interface_.InitializeTcpDriver(); + } } - } - if(rt != Status::ERROR_1){ - try{ - std::vector thread_pool{}; + if (rt != Status::ERROR_1) { + try { + std::vector thread_pool{}; thread_pool.emplace_back([this] { auto result = hw_interface_.GetInventory(); RCLCPP_INFO_STREAM(get_logger(), result); @@ -50,155 +48,82 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) th.join(); } - } - catch (...) - { - std::cout << "catch (...) in parent" << std::endl; - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); - } - if (this->setup_sensor) { - hw_interface_.CheckAndSetConfig(); - updateParameters(); + } catch (...) { + std::cout << "catch (...) in parent" << std::endl; + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); + } + if (this->setup_sensor) { + hw_interface_.CheckAndSetConfig(); + updateParameters(); + } + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); + hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); } } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr_->sensor_model); - hw_interface_.SetTargetModel(sensor_cfg_ptr_->sensor_model); - } -#endif - - hw_interface_.RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - pandar_scan_pub_ = - this->create_publisher("pandar_packets", rclcpp::SensorDataQoS()); - -#if not defined(TEST_PCAP) - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); - } -#endif - - RCLCPP_DEBUG(this->get_logger(), "Starting stream"); - StreamStart(); -} // decoder { - drivers::HesaiCalibrationConfiguration calibration_configuration; - drivers::HesaiCorrection correction_configuration; - - wrapper_status_ = - GetCalibrationData(calibration_configuration, correction_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - calibration_cfg_ptr_ = - std::make_shared(calibration_configuration); + drivers::HesaiCalibrationConfiguration calibration_configuration; + drivers::HesaiCorrection correction_configuration; + wrapper_status_ = GetCalibrationData(calibration_configuration, correction_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { + calibration_cfg_ptr_ = + std::make_shared(calibration_configuration); + // Will be left empty for AT128, and ignored by all decoders except AT128 correction_cfg_ptr_ = std::make_shared(correction_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); wrapper_status_ = InitializeCloudDriver( std::static_pointer_cast(sensor_cfg_ptr_), std::static_pointer_cast(calibration_cfg_ptr_), std::static_pointer_cast(correction_cfg_ptr_)); - } else { - wrapper_status_ = InitializeCloudDriver( - std::static_pointer_cast(sensor_cfg_ptr_), - std::static_pointer_cast(calibration_cfg_ptr_)); - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; - auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), - qos_profile); - - nebula_points_pub_ = - this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); - aw_points_base_pub_ = - this->create_publisher("aw_points", rclcpp::SensorDataQoS()); - aw_points_ex_pub_ = - this->create_publisher("aw_points_ex", rclcpp::SensorDataQoS()); -} + RCLCPP_DEBUG(this->get_logger(), "Starting stream"); + StreamStart(); + hw_interface_.RegisterScanCallback( + std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); -//hwmon -{ -cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); - cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_)); + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_); - message_sep = ": "; - not_supported_message = "Not supported"; - error_message = "Error"; + packet_pub_ = create_publisher( + "hesai_packets", rclcpp::SensorDataQoS()); - switch (sensor_cfg_ptr_->sensor_model) { - case nebula::drivers::SensorModel::HESAI_PANDARXT32: - case nebula::drivers::SensorModel::HESAI_PANDARXT32M: - case nebula::drivers::SensorModel::HESAI_PANDARAT128: - temperature_names.emplace_back("Bottom circuit board T1"); - temperature_names.emplace_back("Bottom circuit board T2"); - temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); - temperature_names.emplace_back("Laser emitting board RT_L2"); - temperature_names.emplace_back("Receiving board RT_R"); - temperature_names.emplace_back("Receiving board RT2"); - temperature_names.emplace_back("Top circuit RT3"); - temperature_names.emplace_back("Not used"); - break; - case nebula::drivers::SensorModel::HESAI_PANDAR64: - case nebula::drivers::SensorModel::HESAI_PANDAR40P: - case nebula::drivers::SensorModel::HESAI_PANDAR40M: - case nebula::drivers::SensorModel::HESAI_PANDARQT64: - case nebula::drivers::SensorModel::HESAI_PANDARQT128: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: - case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: - default: - temperature_names.emplace_back("Bottom circuit RT1"); - temperature_names.emplace_back("Bottom circuit RT2"); - temperature_names.emplace_back("Internal Temperature"); - temperature_names.emplace_back("Laser emitting board RT1"); - temperature_names.emplace_back("Laser emitting board RT2"); - temperature_names.emplace_back("Receiving board RT1"); - temperature_names.emplace_back("Top circuit RT1"); - temperature_names.emplace_back("Top circuit RT2"); - break; - } + packet_sub_ = create_subscription( + "hesai_packets", rclcpp::SensorDataQoS(), [](nebula_msgs::msg::RawPacketStamped::UniquePtr){}); - std::vector thread_pool{}; - thread_pool.emplace_back([this] { - auto result = hw_interface_.GetInventory(); - current_inventory.reset(new HesaiInventory(result)); - current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); - std::cout << "HesaiInventory" << std::endl; - std::cout << result << std::endl; - info_model = result.get_str_model(); - info_serial = std::string(result.sn.begin(), result.sn.end()); - hw_interface_.SetTargetModel(result.model); - RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); - RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); - InitializeHesaiDiagnostics(); - }); - for (std::thread & th : thread_pool) { - th.join(); + nebula_points_pub_ = this->create_publisher( + "pandar_points", rclcpp::SensorDataQoS()); + aw_points_base_pub_ = + this->create_publisher("aw_points", rclcpp::SensorDataQoS()); + aw_points_ex_pub_ = this->create_publisher( + "aw_points_ex", rclcpp::SensorDataQoS()); } + InitializeHwMonitor(*sensor_cfg_ptr_); + set_param_res_ = this->add_on_set_parameters_callback( std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1)); } -} -void HesaiRosWrapper::ReceiveCloudPacketCallback( - const std::vector & packet) +void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector & packet) { + auto t_received = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + auto msg_ptr = std::make_unique(packet.size()); + msg_ptr->stamp.sec = t_received / 1'000'000'000; + msg_ptr->stamp.nanosec = t_received % 1'000'000'000; + // TODO(mojomex) this copy could be avoided if transport_drivers would give us a non-const vector + std::copy(packet.begin(), packet.end(), msg_ptr->data.begin()); + packet_pub_->publish(std::move(msg_ptr)); + + //todo // Driver is not initialized yet if (!driver_ptr_) { return; @@ -209,6 +134,12 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback( driver_ptr_->ParseCloudPacket(packet); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); + auto t_end = std::chrono::high_resolution_clock::now(); + auto runtime = t_end - t_start; + t_start = t_end; + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_decode_packet': %lu}", runtime.count()); + if (pointcloud == nullptr) { // todo // RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); @@ -237,8 +168,8 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback( if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, std::get<1>(pointcloud_ts)); + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = @@ -246,8 +177,9 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback( PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); } - auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); + runtime = std::chrono::high_resolution_clock::now() - t_start; + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_convert_pc': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); } void HesaiRosWrapper::PublishCloud( @@ -261,22 +193,11 @@ void HesaiRosWrapper::PublishCloud( publisher->publish(std::move(pointcloud)); } -Status HesaiRosWrapper::InitializeCloudDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) -{ - driver_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration), - std::static_pointer_cast(calibration_configuration)); - return driver_ptr_->GetStatus(); -} - Status HesaiRosWrapper::InitializeCloudDriver( const std::shared_ptr & sensor_configuration, const std::shared_ptr & calibration_configuration, const std::shared_ptr & correction_configuration) { - // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), std::static_pointer_cast(calibration_configuration), @@ -284,11 +205,12 @@ Status HesaiRosWrapper::InitializeCloudDriver( return driver_ptr_->GetStatus(); } -Status HesaiRosWrapper::GetStatus() { return wrapper_status_; } +Status HesaiRosWrapper::GetStatus() +{ + return wrapper_status_; +} -/// decoder -Status HesaiRosWrapper::GetParameters( - drivers::HesaiSensorConfiguration & sensor_configuration) +Status HesaiRosWrapper::GetParameters(drivers::HesaiSensorConfiguration & sensor_configuration) { /////////////////////////////////////////////// // Define and get ROS parameters @@ -377,8 +299,7 @@ Status HesaiRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("calibration_file", "", descriptor); - calibration_file_path = - this->get_parameter("calibration_file").as_string(); + calibration_file_path = this->get_parameter("calibration_file").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -492,16 +413,6 @@ Status HesaiRosWrapper::GetParameters( this->declare_parameter("setup_sensor", true, descriptor); this->setup_sensor = this->get_parameter("setup_sensor").as_bool(); } - - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_hw_ms", 0, descriptor); - this->delay_hw_ms_ = this->get_parameter("delay_hw_ms").as_int(); - } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; @@ -528,9 +439,9 @@ Status HesaiRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("ptp_transport_type", ""); - sensor_configuration.ptp_transport_type = - nebula::drivers::PtpTransportTypeFromString(this->get_parameter("ptp_transport_type").as_string()); - if(static_cast(sensor_configuration.ptp_profile) > 0) { + sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportTypeFromString( + this->get_parameter("ptp_transport_type").as_string()); + if (static_cast(sensor_configuration.ptp_profile) > 0) { sensor_configuration.ptp_transport_type = nebula::drivers::PtpTransportType::L2; } } @@ -565,32 +476,27 @@ Status HesaiRosWrapper::GetParameters( this->diag_span_ = this->get_parameter("diag_span").as_int(); } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = "milliseconds"; - this->declare_parameter("delay_monitor_ms", 0, descriptor); - this->delay_monitor_ms_ = this->get_parameter("delay_monitor_ms").as_int(); - } - /////////////////////////////////////////////// // Validate ROS parameters /////////////////////////////////////////////// - if(sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { - RCLCPP_ERROR_STREAM(get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); + if (sensor_configuration.ptp_profile == nebula::drivers::PtpProfile::PROFILE_UNKNOWN) { + RCLCPP_ERROR_STREAM( + get_logger(), "Invalid PTP Profile Provided. Please use '1588v2', '802.1as' or 'automotive'"); return Status::SENSOR_CONFIG_ERROR; } - if(sensor_configuration.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 ( + sensor_configuration.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(sensor_configuration.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 (sensor_configuration.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 (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -603,69 +509,61 @@ Status HesaiRosWrapper::GetParameters( return Status::SENSOR_CONFIG_ERROR; } - /////////////////////////////////////////////// - // Decoder-specific setup - /////////////////////////////////////////////// - std::shared_ptr sensor_cfg_ptr = std::make_shared(sensor_configuration); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - /////////////////////////////////////////////// - // HW Interface specific setup - /////////////////////////////////////////////// - - /////////////////////////////////////////////// - // HW Monitor specific setup - /////////////////////////////////////////////// - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); return Status::OK; } Status HesaiRosWrapper::GetCalibrationData( drivers::HesaiCalibrationConfiguration & calibration_configuration, - drivers::HesaiCorrection & correction_configuration) { - calibration_configuration.calibration_file = calibration_file_path; //todo + drivers::HesaiCorrection & correction_configuration) +{ + calibration_configuration.calibration_file = calibration_file_path; // todo - bool run_local = !launch_hw_; + bool run_local = !launch_hw_; if (sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { std::string calibration_file_path_from_sensor; if (launch_hw_ && !calibration_configuration.calibration_file.empty()) { int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); + calibration_file_path_from_sensor += + calibration_configuration.calibration_file.substr(0, ext_pos); calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr( + ext_pos, calibration_configuration.calibration_file.size() - ext_pos); } - if(launch_hw_) { + if (launch_hw_) { run_local = false; RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_cfg_ptr_->sensor_ip << "'"); - std::future future = std::async(std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - auto str = hw_interface_.GetLidarCalibrationString(); - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - RCLCPP_ERROR_STREAM(get_logger(), str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" - << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" - << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), - "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), - "LoadFromString failed:" << str << "\n"); - } - }); + this->get_logger(), + "Trying to acquire calibration data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); + std::future future = std::async( + std::launch::async, + [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { + auto str = hw_interface_.GetLidarCalibrationString(); + auto rt = + calibration_configuration.SaveFileFromString(calibration_file_path_from_sensor, str); + RCLCPP_ERROR_STREAM(get_logger(), str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); + } + }); std::future_status status; status = future.wait_for(std::chrono::milliseconds(5000)); if (status == std::future_status::timeout) { @@ -674,36 +572,38 @@ Status HesaiRosWrapper::GetCalibrationData( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_cfg_ptr_->sensor_ip << "'"); + this->get_logger(), + "Acquired calibration data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "The calibration has been saved in '" << calibration_file_path_from_sensor << "'"); } } - if(run_local) { + if (run_local) { RCLCPP_WARN_STREAM(get_logger(), "Running locally"); bool run_org = false; if (calibration_file_path_from_sensor.empty()) { run_org = true; } else { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_file_path_from_sensor); - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + RCLCPP_INFO_STREAM( + get_logger(), "Trying to load file: " << calibration_file_path_from_sensor); + auto cal_status = calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_file_path_from_sensor << "'"); } } - if(run_org) { - RCLCPP_INFO_STREAM(get_logger(),"Trying to load file: " << calibration_configuration.calibration_file); + if (run_org) { + RCLCPP_INFO_STREAM( + get_logger(), "Trying to load file: " << calibration_configuration.calibration_file); if (calibration_configuration.calibration_file.empty()) { RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = @@ -714,54 +614,61 @@ Status HesaiRosWrapper::GetCalibrationData( this->get_logger(), "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_configuration.calibration_file << "'"); } } } } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 + } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 std::string correction_file_path_from_sensor; if (launch_hw_ && !correction_file_path.empty()) { int ext_pos = correction_file_path.find_last_of('.'); correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); + correction_file_path_from_sensor += + correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { - if (launch_hw_) { - RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor"); - auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); + std::future future = std::async( + std::launch::async, + [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { + if (launch_hw_) { + RCLCPP_INFO_STREAM(this->get_logger(), "Trying to acquire calibration data from sensor"); + auto received_bytes = hw_interface_.GetLidarCalibrationBytes(); + RCLCPP_INFO_STREAM( + get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); + auto rt = correction_configuration.SaveFileFromBinary( + correction_file_path_from_sensor, received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromBinary failed:" << correction_file_path_from_sensor + << ". Falling back to offline calibration file."); + run_local = true; + } + rt = correction_configuration.LoadFromBinary(received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), "LoadFromBinary success" + << "\n"); + run_local = false; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "LoadFromBinary failed" + << ". Falling back to offline calibration file."); + run_local = true; + } + } else { + RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); run_local = true; } - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "Falling back to offline calibration file."); - run_local = true; - } - }); + }); if (!run_local) { std::future_status status; status = future.wait_for(std::chrono::milliseconds(8000)); @@ -770,30 +677,29 @@ Status HesaiRosWrapper::GetCalibrationData( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_cfg_ptr_->sensor_ip << "'"); + this->get_logger(), + "Acquired correction data from sensor: '" << sensor_cfg_ptr_->sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "The correction has been saved in '" << correction_file_path_from_sensor << "'"); } } - if(run_local) { + if (run_local) { bool run_org = false; if (correction_file_path_from_sensor.empty()) { run_org = true; } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); + auto cal_status = correction_configuration.LoadFromFile(correction_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "Load correction data from: '" << correction_file_path_from_sensor << "'"); } } - if(run_org) { + if (run_org) { if (correction_file_path.empty()) { RCLCPP_ERROR_STREAM( this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); @@ -805,20 +711,20 @@ Status HesaiRosWrapper::GetCalibrationData( RCLCPP_ERROR_STREAM( this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); + this->get_logger(), "Load correction data from: '" << correction_file_path << "'"); } } } } - } // end AT128 + } // end AT128 return Status::OK; - } +} -HesaiRosWrapper::~HesaiRosWrapper() { +HesaiRosWrapper::~HesaiRosWrapper() +{ RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); hw_interface_.FinalizeTcpDriver(); } @@ -831,8 +737,14 @@ Status HesaiRosWrapper::StreamStart() return interface_status_; } -Status HesaiRosWrapper::StreamStop() { return Status::OK; } -Status HesaiRosWrapper::Shutdown() { return Status::OK; } +Status HesaiRosWrapper::StreamStop() +{ + return Status::OK; +} +Status HesaiRosWrapper::Shutdown() +{ + return Status::OK; +} Status HesaiRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) @@ -844,8 +756,63 @@ Status HesaiRosWrapper::InitializeHwInterface( // todo: don't think this is nee } Status HesaiRosWrapper::InitializeHwMonitor( // todo: don't think this is needed - const drivers::SensorConfigurationBase & sensor_configuration) + const drivers::SensorConfigurationBase & /* sensor_configuration */) { + cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return Status::ERROR_1; + } + + message_sep = ": "; + not_supported_message = "Not supported"; + error_message = "Error"; + + switch (sensor_cfg_ptr_->sensor_model) { + case nebula::drivers::SensorModel::HESAI_PANDARXT32: + case nebula::drivers::SensorModel::HESAI_PANDARXT32M: + case nebula::drivers::SensorModel::HESAI_PANDARAT128: + temperature_names.emplace_back("Bottom circuit board T1"); + temperature_names.emplace_back("Bottom circuit board T2"); + temperature_names.emplace_back("Laser emitting board RT_L1(Internal)"); + temperature_names.emplace_back("Laser emitting board RT_L2"); + temperature_names.emplace_back("Receiving board RT_R"); + temperature_names.emplace_back("Receiving board RT2"); + temperature_names.emplace_back("Top circuit RT3"); + temperature_names.emplace_back("Not used"); + break; + case nebula::drivers::SensorModel::HESAI_PANDAR64: + case nebula::drivers::SensorModel::HESAI_PANDAR40P: + case nebula::drivers::SensorModel::HESAI_PANDAR40M: + case nebula::drivers::SensorModel::HESAI_PANDARQT64: + case nebula::drivers::SensorModel::HESAI_PANDARQT128: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X: + case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X: + default: + temperature_names.emplace_back("Bottom circuit RT1"); + temperature_names.emplace_back("Bottom circuit RT2"); + temperature_names.emplace_back("Internal Temperature"); + temperature_names.emplace_back("Laser emitting board RT1"); + temperature_names.emplace_back("Laser emitting board RT2"); + temperature_names.emplace_back("Receiving board RT1"); + temperature_names.emplace_back("Top circuit RT1"); + temperature_names.emplace_back("Top circuit RT2"); + break; + } + + auto result = hw_interface_.GetInventory(); + current_inventory.reset(new HesaiInventory(result)); + current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now())); + std::cout << "HesaiInventory" << std::endl; + std::cout << result << std::endl; + info_model = result.get_str_model(); + info_serial = std::string(result.sn.begin(), result.sn.end()); + hw_interface_.SetTargetModel(result.model); + RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model); + RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial); + InitializeHesaiDiagnostics(); return Status::OK; } @@ -858,16 +825,14 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback( RCLCPP_DEBUG_STREAM(this->get_logger(), *sensor_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), p); - std::shared_ptr new_param = + std::shared_ptr new_param = std::make_shared(*sensor_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), new_param); std::string sensor_model_str; std::string return_mode_str; if ( - get_param(p, "sensor_model", sensor_model_str) | - get_param(p, "return_mode", return_mode_str) | - get_param(p, "host_ip", new_param->host_ip) | - get_param(p, "sensor_ip", new_param->sensor_ip) | + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | + get_param(p, "host_ip", new_param->host_ip) | get_param(p, "sensor_ip", new_param->sensor_ip) | get_param(p, "frame_id", new_param->frame_id) | get_param(p, "data_port", new_param->data_port) | get_param(p, "gnss_port", new_param->gnss_port) | @@ -938,8 +903,7 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() diagnostics_updater_.add("hesai_status", this, &HesaiRosWrapper::HesaiCheckStatus); diagnostics_updater_.add("hesai_ptp", this, &HesaiRosWrapper::HesaiCheckPtp); - diagnostics_updater_.add( - "hesai_temperature", this, &HesaiRosWrapper::HesaiCheckTemperature); + diagnostics_updater_.add("hesai_temperature", this, &HesaiRosWrapper::HesaiCheckTemperature); diagnostics_updater_.add("hesai_rpm", this, &HesaiRosWrapper::HesaiCheckRpm); current_status.reset(); @@ -956,8 +920,7 @@ void HesaiRosWrapper::InitializeHesaiDiagnostics() this->get_node_timers_interface()->add_timer(diagnostics_status_timer_, cbg_m_); if (hw_interface_.UseHttpGetLidarMonitor()) { - diagnostics_updater_.add( - "hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); + diagnostics_updater_.add("hesai_voltage", this, &HesaiRosWrapper::HesaiCheckVoltageHttp); auto on_timer_lidar_monitor = [this] { OnHesaiLidarMonitorTimerHttp(); }; diagnostics_lidar_monitor_timer_ = std::make_shared>( @@ -1034,12 +997,10 @@ void HesaiRosWrapper::OnHesaiStatusTimer() current_status.reset(new HesaiLidarStatus(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(std::system_error)"), - error.what()); + rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(std::system_error)"), error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + rclcpp::get_logger("HesaiRosWrapper::OnHesaiStatusTimer(boost::system::system_error)"), error.what()); } } @@ -1056,8 +1017,7 @@ void HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp() }); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( @@ -1082,14 +1042,12 @@ void HesaiRosWrapper::OnHesaiLidarMonitorTimer() error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger( - "HesaiRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), + rclcpp::get_logger("HesaiRosWrapper::OnHesaiLidarMonitorTimer(boost::system::system_error)"), error.what()); } } -void HesaiRosWrapper::HesaiCheckStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_status); if (current_status) { @@ -1106,8 +1064,7 @@ void HesaiRosWrapper::HesaiCheckStatus( } } -void HesaiRosWrapper::HesaiCheckPtp( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_status); if (current_status) { @@ -1157,8 +1114,7 @@ void HesaiRosWrapper::HesaiCheckTemperature( } } -void HesaiRosWrapper::HesaiCheckRpm( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_status); if (current_status) { @@ -1205,8 +1161,7 @@ void HesaiRosWrapper::HesaiCheckVoltageHttp( } } -void HesaiRosWrapper::HesaiCheckVoltage( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiRosWrapper::HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_monitor); if (current_monitor) { @@ -1225,6 +1180,6 @@ void HesaiRosWrapper::HesaiCheckVoltage( } } - RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) +RCLCPP_COMPONENTS_REGISTER_NODE(HesaiRosWrapper) } // namespace ros } // namespace nebula diff --git a/scripts/plot_times.py b/scripts/plot_times.py index 521dfd347..3cbf93f61 100644 --- a/scripts/plot_times.py +++ b/scripts/plot_times.py @@ -24,7 +24,6 @@ def parse_logs(run_name): for col in [c for c in df.columns if c.startswith("d_")]: df[col] /= 1e6 # ns to ms - df['d_total'] = sum([df[c] for c in df.columns if c.startswith("d_")]) # type: ignore return df def plot_timing_comparison(run_names): @@ -41,11 +40,13 @@ def plot_timing_comparison(run_names): boxes = axs[1:] for i, (label, df) in enumerate(scenario_dfs.items()): - durations = df['d_total'] + # durations = df['d_total'] - ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') + # ax_d.plot(durations.index, durations, label=label, linestyle='', marker='.') for col in filter(lambda col: col.startswith("n_"), df.columns): ax_n.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.', color='black') + for col in filter(lambda col: col.startswith("d_"), df.columns): + ax_d.plot(df.index, df[col], label=f"{label}::{col}", linestyle='', marker='.') d_columns = [col for col in df.columns if col.startswith("d_")] n_cols = len(d_columns)