From 5d77587e9c81645324611b31c8ae8e39a6033f81 Mon Sep 17 00:00:00 2001
From: Riccardo Bertoglio <riccardo.bertoglio@mail.polimi.it>
Date: Fri, 27 Mar 2020 19:13:44 +0100
Subject: [PATCH] parameter to specify odom target frame

---
 apps/hdl_localization_nodelet.cpp | 23 ++++++++++++++++++-----
 launch/hdl_localization.launch    |  7 +++++++
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/apps/hdl_localization_nodelet.cpp b/apps/hdl_localization_nodelet.cpp
index e0871138..58a8a0ae 100644
--- a/apps/hdl_localization_nodelet.cpp
+++ b/apps/hdl_localization_nodelet.cpp
@@ -5,6 +5,7 @@
 
 #include <ros/ros.h>
 #include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
 #include <tf_conversions/tf_eigen.h>
 #include <tf/transform_broadcaster.h>
 
@@ -42,6 +43,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
 
     processing_time.resize(16);
     initialize_params();
+    
+    odom_child_frame_id = private_nh.param<std::string>("odom_child_frame_id", "base_link");
 
     use_imu = private_nh.param<bool>("use_imu", true);
     invert_imu = private_nh.param<bool>("invert_imu", false);
@@ -127,14 +130,21 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
     }
 
     const auto& stamp = points_msg->header.stamp;
-    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
-    pcl::fromROSMsg(*points_msg, *cloud);
+    pcl::PointCloud<PointT>::Ptr pcl_cloud(new pcl::PointCloud<PointT>());
+    pcl::fromROSMsg(*points_msg, *pcl_cloud);
 
-    if(cloud->empty()) {
+    if(pcl_cloud->empty()) {
       NODELET_ERROR("cloud is empty!!");
       return;
     }
 
+    // transform pointcloud into odom_child_frame_id  
+    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  
+    if(!pcl_ros::transformPointCloud(odom_child_frame_id, *pcl_cloud, *cloud, this->tf_listener)) {
+        NODELET_ERROR("point cloud cannot be transformed into target frame!!");
+        return;
+    } 
+
     auto filtered = downsample(cloud);
 
     // predict
@@ -230,7 +240,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
    */
   void publish_odometry(const ros::Time& stamp, const Eigen::Matrix4f& pose) {
     // broadcast the transform over tf
-    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", "velodyne");
+    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "map", odom_child_frame_id);
     pose_broadcaster.sendTransform(odom_trans);
 
     // publish the transform
@@ -243,7 +253,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
     odom.pose.pose.position.z = pose(2, 3);
     odom.pose.pose.orientation = odom_trans.transform.rotation;
 
-    odom.child_frame_id = "velodyne";
+    odom.child_frame_id = odom_child_frame_id;
     odom.twist.twist.linear.x = 0.0;
     odom.twist.twist.linear.y = 0.0;
     odom.twist.twist.angular.z = 0.0;
@@ -286,6 +296,8 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
   ros::NodeHandle nh;
   ros::NodeHandle mt_nh;
   ros::NodeHandle private_nh;
+  
+  std::string odom_child_frame_id;
 
   bool use_imu;
   bool invert_imu;
@@ -297,6 +309,7 @@ class HdlLocalizationNodelet : public nodelet::Nodelet {
   ros::Publisher pose_pub;
   ros::Publisher aligned_pub;
   tf::TransformBroadcaster pose_broadcaster;
+  tf::TransformListener tf_listener;
 
   // imu input buffer
   std::mutex imu_data_mutex;
diff --git a/launch/hdl_localization.launch b/launch/hdl_localization.launch
index bf456ea6..83c61a62 100644
--- a/launch/hdl_localization.launch
+++ b/launch/hdl_localization.launch
@@ -2,6 +2,9 @@
 <launch>
   <!-- arguments -->
   <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
+  <arg name="points_topic" default="/velodyne_points" />
+  <arg name="imu_topic" default="/gpsimu_driver/imu_data" />
+  <arg name="odom_child_frame_id" default="velodyne" />
 
   <!-- in case you use velodyne_driver, comment out the following line -->
   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
@@ -14,6 +17,10 @@
 
     <!-- hdl_localization_nodelet -->
     <node pkg="nodelet" type="nodelet" name="hdl_localization_nodelet" args="load hdl_localization/HdlLocalizationNodelet $(arg nodelet_manager)">
+        <remap from="/velodyne_points" to="$(arg points_topic)" />
+        <remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" />
+        <!-- odometry frame_id -->
+        <param name="odom_child_frame_id" value="$(arg odom_child_frame_id)" />
         <!-- imu settings -->
         <!-- during "cool_time", imu inputs are ignored -->
         <param name="use_imu" value="true" />