From 9f4218e60ae0a2bb06d2a843fb9b8a5948dd1a4a Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Fri, 11 Oct 2024 11:04:55 +0200 Subject: [PATCH] Add intensity field to pointcloud - also updated pcd frame_id to correspond with TFs --- .../micro_epsilon_scancontrol_driver/driver.h | 3 ++- .../src/driver.cpp | 22 +++++++++++-------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h index 4c722de..170bb16 100644 --- a/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h +++ b/micro_epsilon_scancontrol_driver/include/micro_epsilon_scancontrol_driver/driver.h @@ -24,7 +24,7 @@ #define MAX_RESOLUTION_COUNT 6 #define GENERAL_FUNCTION_FAILED -1 -#define DEFAULT_FRAME_ID "scancontrol" +#define DEFAULT_FRAME_ID "laser_frame" #define DEFAULT_TOPIC_NAME "scancontrol_pointcloud" typedef pcl::PointCloud point_cloud_t; @@ -118,6 +118,7 @@ namespace scancontrol_driver std::vector value_x, value_z; int lost_values; unsigned int lost_profiles; + std::vector maximum_intensity, threshold; point_cloud_t::Ptr point_cloud_msg; diff --git a/micro_epsilon_scancontrol_driver/src/driver.cpp b/micro_epsilon_scancontrol_driver/src/driver.cpp index e2360a7..1ff9742 100644 --- a/micro_epsilon_scancontrol_driver/src/driver.cpp +++ b/micro_epsilon_scancontrol_driver/src/driver.cpp @@ -26,14 +26,14 @@ namespace scancontrol_driver private_nh_->get_parameter_or("topic_name", config_.topic_name, std::string(DEFAULT_TOPIC_NAME)); // TODO: Are these parameters needed? - private_nh_->declare_parameter("partial_profile_start_point", 0); - private_nh_->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0); - private_nh_->declare_parameter("partial_profile_start_point_data", 4); - private_nh_->get_parameter_or("partial_profile_start_point_data", config_.pp_start_point_data, 4); - private_nh_->declare_parameter("partial_profile_point_count", -1); - private_nh_->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1); - private_nh_->declare_parameter("partial_profile_data_width", 4); - private_nh_->get_parameter_or("partial_profile_data_width", config_.pp_point_data_width, 4); + this->declare_parameter("partial_profile_start_point", 0); + this->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0); + this->declare_parameter("partial_profile_start_point_data", 0); + this->get_parameter_or("partial_profile_start_point_data", config_.pp_start_point_data, 0); + this->declare_parameter("partial_profile_point_count", -1); + this->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1); + this->declare_parameter("partial_profile_data_width", 8); + this->get_parameter_or("partial_profile_data_width", config_.pp_point_data_width, 8); // Create driver interface object: device_interface_ptr = std::make_unique(); @@ -283,6 +283,9 @@ namespace scancontrol_driver lost_values = (16 + t_partial_profile_.nPointDataWidth - 1)/t_partial_profile_.nPointDataWidth; value_x.resize(t_partial_profile_.nPointCount); value_z.resize(t_partial_profile_.nPointCount); + maximum_intensity.resize(t_partial_profile_.nPointCount); + threshold.resize(t_partial_profile_.nPointCount); + RCLCPP_INFO_STREAM(LOGGER, "Profile is losing " << std::to_string(lost_values) << " values due to timestamp of 16 byte at the end of the profile."); // Prepare new point cloud message @@ -323,7 +326,7 @@ namespace scancontrol_driver /* Process raw profile data and create the point cloud message */ int ScanControlDriver::Profile2PointCloud(){ - device_interface_ptr->ConvertPartProfile2Values(&profile_buffer[0], profile_buffer.size(), &t_partial_profile_, device_type, 0, NULL, NULL, NULL, &value_x[0], &value_z[0], NULL, NULL); + int ret_code = device_interface_ptr->ConvertPartProfile2Values(&profile_buffer[0], profile_buffer.size(), &t_partial_profile_, device_type, 0, NULL, &maximum_intensity[0], &threshold[0], &value_x[0], &value_z[0], NULL, NULL); for (int i = 0; i < config_.resolution; i++){ point_cloud_msg->points[i].x = value_x[i]/1000; @@ -333,6 +336,7 @@ namespace scancontrol_driver } else{ point_cloud_msg->points[i].z = value_z[i]/1000; + point_cloud_msg->points[i].intensity = maximum_intensity[i]; } } return GENERAL_FUNCTION_OK;