Lidar Undistortion with Velodyne driver 1.6.0

[sasupe]

Hi!
Can anyone in the community take a look at the following update to lidar_undistorter.cpp to work with velodyne drivers 1.6.0 which has support for pointcloud of type XYZIRT

The package is compiling and working. I am getting the corrected pointcloud but just wanted to make sure if I have not done any obvious mistake.
Hope this helps who is working with Velodyne Drivers.

Thanks

	#include "lidar_undistorter.h"
	#include <pcl_conversions/pcl_conversions.h>
	#include <velodyne_pcl/point_types.h>
	#include <pcl/PCLPointCloud2.h>
	#include <pcl/conversions.h>
	#include <pcl/common/transforms.h>
	#include <pcl/point_cloud.h>
	#include <string>

	namespace lidar_undistortion {
	LidarUndistorter::LidarUndistorter(ros::NodeHandle nh,
									   ros::NodeHandle nh_private)
		: fixed_frame_id_("map"),
		  lidar_frame_id_("lidar_red"),
		  tf_buffer_(ros::Duration(10)),
		  tf_listener_(tf_buffer_) {
	  // Subscribe to the undistorted pointcloud topic
	  pointcloud_sub_ = nh.subscribe("lidar_red_pointcloud", 100,&LidarUndistorter::pointcloudCallback, this);

	  // Advertise the corrected pointcloud topic
	  corrected_pointcloud_pub_ = nh_private.advertise<sensor_msgs::PointCloud2>("pointcloud_corrected", 100, false);

	  // Read the odom and lidar frame names from ROS params
	  nh_private.param("odom_frame_id", fixed_frame_id_, fixed_frame_id_);
	  nh_private.param("lidar_frame_id", lidar_frame_id_, lidar_frame_id_);
	}

	void LidarUndistorter::pointcloudCallback(const sensor_msgs::PointCloud2 &pointcloud_msg) {
		// Convert the pointcloud to PCL

		pcl::PointCloud<velodyne_pcl::PointXYZIRT> pointcloud;
		pcl::fromROSMsg(pointcloud_msg, pointcloud);

		if (pointcloud.empty()) return;
	  
		// Get the start and end times of the pointcloud
		ros::Time t_start = pointcloud_msg.header.stamp + ros::Duration(pointcloud.points.begin()->time);
		ros::Time t_end = pointcloud_msg.header.stamp + ros::Duration((--pointcloud.points.end())->time);

		try {
			// Wait for all transforms to become available
			if (!waitForTransform(lidar_frame_id_, fixed_frame_id_, t_end, 0.05, 0.25)) {
			  ROS_WARN(
				  "Could not get correction transform within allotted time. "
				  "Skipping pointcloud.");
			  return;
			}

			// Get the frame that the cloud should be expressed in
			geometry_msgs::TransformStamped msg_T_F_S_original = tf_buffer_.lookupTransform(fixed_frame_id_, lidar_frame_id_, t_start);
			Eigen::Affine3f T_F_S_original;
			transformMsgToEigen(msg_T_F_S_original.transform, T_F_S_original);

			// Compute the transform used to project the corrected pointcloud back into
			// lidar's scan frame, for more info see the current class' header
			Eigen::Affine3f T_S_F_original = T_F_S_original.inverse();

			// Correct the distortion on all points, using the LiDAR's true pose at
			// each point's timestamp
			uint32_t last_transform_update_t = 0;
			Eigen::Affine3f T_S_original__S_corrected = Eigen::Affine3f::Identity();
		  
			for (velodyne_pcl::PointXYZIRT &point : pointcloud.points) {
				// Check if the current point's timestamp differs from the previous one
				// If so, lookup the new corresponding transform
				if (point.time != last_transform_update_t) {
					last_transform_update_t = point.time;
					ros::Time point_t = pointcloud_msg.header.stamp + ros::Duration(point.time);
					//std::cout.precision(17);
					//std::cout<<point_t.toSec()<<std::fixed<<"\n";
					geometry_msgs::TransformStamped msg_T_F_S_correct = tf_buffer_.lookupTransform(fixed_frame_id_, lidar_frame_id_,point_t);
					Eigen::Affine3f T_F_S_correct;
					transformMsgToEigen(msg_T_F_S_correct.transform, T_F_S_correct);
					T_S_original__S_corrected = T_S_F_original * T_F_S_correct;
				}
				//std::cout<<point.time<<"\n";
				// Correct the point's distortion, by transforming it into the fixed
				// frame based on the LiDAR sensor's current true pose, and then transform
				// it back into the lidar scan frame
				point = pcl::transformPoint(point, T_S_original__S_corrected);
			}
		} 
		catch (tf2::TransformException &ex) {
			ROS_WARN("%s", ex.what());
			return;
		}
		
		// Create the corrected pointcloud ROS msg
		sensor_msgs::PointCloud2 pointcloud_corrected_msg;
		pcl::toROSMsg(pointcloud, pointcloud_corrected_msg);

		// Copy the pointcloud header correctly
		// NOTE: The header timestamp type in PCL pointclouds is narrower than in
		//       PointCloud2 msgs. We therefore copy this field directly from the
		//       losing timestamp accuracy.
		pointcloud_corrected_msg.header = pointcloud_msg.header;

		// Publish the corrected pointcloud
		corrected_pointcloud_pub_.publish(pointcloud_corrected_msg);


	}

	bool LidarUndistorter::waitForTransform(const std::string &from_frame_id,
											const std::string &to_frame_id,
											const ros::Time &frame_timestamp,
											const double &sleep_between_retries__s,
											const double &timeout__s) {
	  // Total time spent waiting for the updated pose
	  ros::WallDuration t_waited(0.0);
	  // Maximum time to wait before giving up
	  ros::WallDuration t_max(timeout__s);
	  // Timeout between each update attempt
	  const ros::WallDuration t_sleep(sleep_between_retries__s);
	  while (t_waited < t_max) {
		if (tf_buffer_.canTransform(to_frame_id, from_frame_id, frame_timestamp)) {
		  return true;
		}
		t_sleep.sleep();
		t_waited += t_sleep;
	  }
	  ROS_WARN("Waited %.3fs, but still could not get the TF from %s to %s",
			   t_waited.toSec(), from_frame_id.c_str(), to_frame_id.c_str());
	  return false;
	}
	}  // namespace lidar_undistortion