Add intensity field to pointcloud

- also updated pcd frame_id to correspond with TFs
sam-xl · Oct 11, 2024 · 9f4218e · 9f4218e
1 parent 12940b7
commit 9f4218e
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 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
@@ -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<pcl::PointXYZI> point_cloud_t;
@@ -118,6 +118,7 @@ namespace scancontrol_driver
             std::vector<double> value_x, value_z;
             int lost_values;
             unsigned int lost_profiles;
+            std::vector<unsigned short> 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
@@ -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<int>("partial_profile_start_point", 0);
-        private_nh_->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0);
-        private_nh_->declare_parameter<int>("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<int>("partial_profile_point_count", -1);
-        private_nh_->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1);
-        private_nh_->declare_parameter<int>("partial_profile_data_width", 4);
-        private_nh_->get_parameter_or("partial_profile_data_width", config_.pp_point_data_width, 4);
+        this->declare_parameter<int>("partial_profile_start_point", 0);
+        this->get_parameter_or("partial_profile_start_point", config_.pp_start_point, 0);
+        this->declare_parameter<int>("partial_profile_start_point_data", 0);
+        this->get_parameter_or("partial_profile_start_point_data", config_.pp_start_point_data, 0);
+        this->declare_parameter<int>("partial_profile_point_count", -1);
+        this->get_parameter_or("partial_profile_point_count", config_.pp_point_count, -1);
+        this->declare_parameter<int>("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<CInterfaceLLT>();
@@ -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;