[Feature Proposal] Add map to odom TF publishing capability (#62)

* 変更：PCDマップ読み取りの場所で、PLYファイルも読み取れるようにした

* 追加：map->odomのTF変換モードを追加

Author: abudori3939

include/lidar_localization/lidar_localization_component.hpp

@@ -122,6 +122,7 @@ class PCLLocalization : public rclcpp_lifecycle::LifecycleNode
   double last_odom_received_time_;
   bool use_imu_{false};
   bool enable_debug_{false};
+  bool enable_map_odom_tf_{false};
 
   int ndt_num_threads_;
   int ndt_max_iterations_;

param/localization.yaml

@@ -24,6 +24,7 @@
       use_odom: false
       use_imu: false
       enable_debug: true
+      enable_map_odom_tf: false
       global_frame_id: map
       odom_frame_id: odom
       base_frame_id: base_link

src/lidar_localization_component.cpp

@@ -9,6 +9,7 @@ PCLLocalization::PCLLocalization(const rclcpp::NodeOptions & options)
   declare_parameter("global_frame_id", "map");
   declare_parameter("odom_frame_id", "odom");
   declare_parameter("base_frame_id", "base_link");
+  declare_parameter("enable_map_odom_tf", false);
   declare_parameter("registration_method", "NDT");
   declare_parameter("score_threshold", 2.0);
   declare_parameter("ndt_resolution", 1.0);
@@ -174,6 +175,7 @@ void PCLLocalization::initializeParameters()
   get_parameter("global_frame_id", global_frame_id_);
   get_parameter("odom_frame_id", odom_frame_id_);
   get_parameter("base_frame_id", base_frame_id_);
+  get_parameter("enable_map_odom_tf", enable_map_odom_tf_);
   get_parameter("registration_method", registration_method_);
   get_parameter("score_threshold", score_threshold_);
   get_parameter("ndt_resolution", ndt_resolution_);
@@ -202,6 +204,7 @@ void PCLLocalization::initializeParameters()
   RCLCPP_INFO(get_logger(),"global_frame_id: %s", global_frame_id_.c_str());
   RCLCPP_INFO(get_logger(),"odom_frame_id: %s", odom_frame_id_.c_str());
   RCLCPP_INFO(get_logger(),"base_frame_id: %s", base_frame_id_.c_str());
+  RCLCPP_INFO(get_logger(),"enable_map_odom_tf: %d", enable_map_odom_tf_);
   RCLCPP_INFO(get_logger(),"registration_method: %s", registration_method_.c_str());
   RCLCPP_INFO(get_logger(),"ndt_resolution: %lf", ndt_resolution_);
   RCLCPP_INFO(get_logger(),"ndt_step_size: %lf", ndt_step_size_);
@@ -501,15 +504,40 @@ void PCLLocalization::cloudReceived(const sensor_msgs::msg::PointCloud2::ConstSh
   corrent_pose_with_cov_stamped_ptr_->pose.pose.orientation = quat_msg;
   pose_pub_->publish(*corrent_pose_with_cov_stamped_ptr_);
 
-  geometry_msgs::msg::TransformStamped transform_stamped;
-  transform_stamped.header.stamp = msg->header.stamp;
-  transform_stamped.header.frame_id = global_frame_id_;
-  transform_stamped.child_frame_id = base_frame_id_;
-  transform_stamped.transform.translation.x = static_cast<double>(final_transformation(0, 3));
-  transform_stamped.transform.translation.y = static_cast<double>(final_transformation(1, 3));
-  transform_stamped.transform.translation.z = static_cast<double>(final_transformation(2, 3));
-  transform_stamped.transform.rotation = quat_msg;
-  broadcaster_.sendTransform(transform_stamped);
+  geometry_msgs::msg::TransformStamped map_to_base_link_stamped;
+  map_to_base_link_stamped.header.stamp = msg->header.stamp;
+  map_to_base_link_stamped.header.frame_id = global_frame_id_;
+  map_to_base_link_stamped.child_frame_id = base_frame_id_;
+  map_to_base_link_stamped.transform.translation.x = static_cast<double>(final_transformation(0, 3));
+  map_to_base_link_stamped.transform.translation.y = static_cast<double>(final_transformation(1, 3));
+  map_to_base_link_stamped.transform.translation.z = static_cast<double>(final_transformation(2, 3));
+  map_to_base_link_stamped.transform.rotation = quat_msg;
+  if (!enable_map_odom_tf_) {
+    broadcaster_.sendTransform(map_to_base_link_stamped);
+  } else {
+    tf2::Transform map_to_base_link_tf;
+    tf2::fromMsg(map_to_base_link_stamped.transform, map_to_base_link_tf);
+
+    geometry_msgs::msg::TransformStamped odom_to_base_link_msg;
+    try {
+      odom_to_base_link_msg = tfbuffer_.lookupTransform(
+        odom_frame_id_, base_frame_id_, msg->header.stamp, rclcpp::Duration::from_seconds(0.1));
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_WARN(
+        this->get_logger(), "Could not get transform %s to %s: %s",
+        base_frame_id_.c_str(), odom_frame_id_.c_str(), ex.what());
+      return;
+    }
+    tf2::Transform odom_to_base_link_tf;
+    tf2::fromMsg(odom_to_base_link_msg.transform, odom_to_base_link_tf);
+
+    tf2::Transform map_to_odom_tf = map_to_base_link_tf * odom_to_base_link_tf.inverse();
+    geometry_msgs::msg::TransformStamped map_to_odom_stamped;
+    map_to_odom_stamped.header.frame_id = global_frame_id_;
+    map_to_odom_stamped.child_frame_id = odom_frame_id_;
+    map_to_odom_stamped.transform = tf2::toMsg(map_to_odom_tf);
+    broadcaster_.sendTransform(map_to_odom_stamped);
+  }
 
   geometry_msgs::msg::PoseStamped::SharedPtr pose_stamped_ptr(new geometry_msgs::msg::PoseStamped);
   pose_stamped_ptr->header.stamp = msg->header.stamp;