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)