Fix tf broadcasting

beluga_amcl/include/beluga_amcl/amcl_node.hpp

@@ -168,6 +168,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode {
   std::unique_ptr<beluga_ros::Amcl> particle_filter_;
   /// Last known pose estimate, if any.
   std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_;
+  /// Last known map to odom correction estimate, if any.
+  std::optional<Sophus::SE2d> last_known_odom_transform_in_map_;
   /// Whether to broadcast transforms or not.
   bool enable_tf_broadcast_{false};
 };

beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp

@@ -196,6 +196,8 @@ class AmclNodelet : public nodelet::Nodelet {
   std::unique_ptr<beluga_ros::Amcl> particle_filter_;
   /// Last known pose estimate, if any.
   std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_;
+  /// Last known map to odom correction estimate, if any.
+  std::optional<Sophus::SE2d> last_known_odom_transform_in_map_;
   /// Last known occupancy grid map.
   nav_msgs::OccupancyGrid::ConstPtr last_known_map_;
   /// Whether to broadcast transforms or not.

beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp

@@ -184,6 +184,8 @@ class NdtAmclNode : public rclcpp_lifecycle::LifecycleNode {
   std::unique_ptr<NdtAmclVariant> particle_filter_ = nullptr;
   /// Last known pose estimate, if any.
   std::optional<std::pair<Sophus::SE2d, Eigen::Matrix3d>> last_known_estimate_ = std::nullopt;
+  /// Last known map to odom correction estimate, if any.
+  std::optional<Sophus::SE2d> last_known_odom_transform_in_map_;
   /// Whether to broadcast transforms or not.
   bool enable_tf_broadcast_{false};
 };

beluga_amcl/src/amcl_node.cpp

@@ -431,6 +431,7 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) {
     const auto initial_estimate = get_initial_estimate();
     if (initial_estimate.has_value()) {
       last_known_estimate_ = initial_estimate;
+      last_known_odom_transform_in_map_.reset();
     }
   }
 
@@ -509,6 +510,8 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
   const auto update_duration = update_stop_time - update_start_time;
 
   if (new_estimate.has_value()) {
+    const auto& [base_pose_in_map, _] = new_estimate.value();
+    last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse();
     last_known_estimate_ = new_estimate;
 
     RCLCPP_INFO(
@@ -536,15 +539,14 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_
 
   // Transforms are always published to keep them current.
   if (enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) {
-    const auto odom_transform_in_map = base_pose_in_map * base_pose_in_odom.inverse();
     auto message = geometry_msgs::msg::TransformStamped{};
     // Sending a transform that is valid into the future so that odom can be used.
     const auto expiration_stamp = tf2_ros::fromMsg(laser_scan->header.stamp) +
                                   tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
     message.header.stamp = tf2_ros::toMsg(expiration_stamp);
     message.header.frame_id = get_parameter("global_frame_id").as_string();
     message.child_frame_id = get_parameter("odom_frame_id").as_string();
-    message.transform = tf2::toMsg(odom_transform_in_map);
+    message.transform = tf2::toMsg(*last_known_odom_transform_in_map_);
     tf_broadcaster_->sendTransform(message);
   }
 }
@@ -565,6 +567,7 @@ void AmclNode::initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamp
   tf2::covarianceRowMajorToEigen(message->pose.covariance, covariance);
 
   last_known_estimate_ = std::make_pair(pose, covariance);
+  last_known_odom_transform_in_map_.reset();
   initialize_from_estimate(last_known_estimate_.value());
 }
 

beluga_amcl/src/amcl_nodelet.cpp

@@ -309,6 +309,7 @@ bool AmclNodelet::set_map_callback(nav_msgs::SetMap::Request& request, nav_msgs:
   tf2::covarianceRowMajorToEigen(request.initial_pose.pose.covariance, covariance);
 
   last_known_estimate_ = std::make_pair(pose, covariance);
+  last_known_odom_transform_in_map_.reset();
   const bool success = initialize_from_estimate(last_known_estimate_.value());
 
   response.success = static_cast<unsigned char>(success);
@@ -352,6 +353,7 @@ void AmclNodelet::handle_map_with_default_initial_pose(const nav_msgs::Occupancy
     const auto initial_estimate = get_initial_estimate();
     if (initial_estimate.has_value()) {
       last_known_estimate_ = initial_estimate;
+      last_known_odom_transform_in_map_.reset();
     }
   }
 
@@ -436,6 +438,8 @@ void AmclNodelet::laser_callback(const sensor_msgs::LaserScan::ConstPtr& laser_s
   const auto update_duration = update_stop_time - update_start_time;
 
   if (new_estimate.has_value()) {
+    const auto& [base_pose_in_map, _] = new_estimate.value();
+    last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse();
     last_known_estimate_ = new_estimate;
 
     NODELET_INFO(
@@ -464,13 +468,12 @@ void AmclNodelet::laser_callback(const sensor_msgs::LaserScan::ConstPtr& laser_s
 
   // Transforms are always published to keep them current
   if (enable_tf_broadcast_ && config_.tf_broadcast) {
-    const auto odom_transform_in_map = base_pose_in_map * base_pose_in_odom.inverse();
     auto message = geometry_msgs::TransformStamped{};
     // Sending a transform that is valid into the future so that odom can be used.
     message.header.stamp = ros::Time::now() + ros::Duration(config_.transform_tolerance);
     message.header.frame_id = config_.global_frame_id;
     message.child_frame_id = config_.odom_frame_id;
-    message.transform = tf2::toMsg(odom_transform_in_map);
+    message.transform = tf2::toMsg(*last_known_odom_transform_in_map_);
     tf_broadcaster_->sendTransform(message);
   }
 
@@ -498,6 +501,7 @@ void AmclNodelet::initial_pose_callback(const geometry_msgs::PoseWithCovarianceS
   tf2::covarianceRowMajorToEigen(message->pose.covariance, covariance);
 
   last_known_estimate_ = std::make_pair(pose, covariance);
+  last_known_odom_transform_in_map_.reset();
   initialize_from_estimate(last_known_estimate_.value());
 }
 

beluga_amcl/src/ndt_amcl_node.cpp

@@ -160,6 +160,7 @@ NdtAmclNode::CallbackReturn NdtAmclNode::on_activate(const rclcpp_lifecycle::Sta
     const auto initial_estimate = get_initial_estimate();
     if (initial_estimate.has_value()) {
       last_known_estimate_ = initial_estimate;
+      last_known_odom_transform_in_map_.reset();
       initialize_from_estimate(initial_estimate.value());
     }
   }
@@ -395,6 +396,8 @@ void NdtAmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr las
   const auto update_duration = update_stop_time - update_start_time;
 
   if (new_estimate.has_value()) {
+    const auto& [base_pose_in_map, _] = new_estimate.value();
+    last_known_odom_transform_in_map_ = base_pose_in_map * base_pose_in_odom.inverse();
     last_known_estimate_ = new_estimate;
 
     const auto num_particles =
@@ -424,15 +427,14 @@ void NdtAmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr las
 
   // Transforms are always published to keep them current.
   if (enable_tf_broadcast_ && get_parameter("tf_broadcast").as_bool()) {
-    const auto odom_transform_in_map = base_pose_in_map * base_pose_in_odom.inverse();
     auto message = geometry_msgs::msg::TransformStamped{};
     // Sending a transform that is valid into the future so that odom can be used.
     const auto expiration_stamp = tf2_ros::fromMsg(laser_scan->header.stamp) +
                                   tf2::durationFromSec(get_parameter("transform_tolerance").as_double());
     message.header.stamp = tf2_ros::toMsg(expiration_stamp);
     message.header.frame_id = get_parameter("global_frame_id").as_string();
     message.child_frame_id = get_parameter("odom_frame_id").as_string();
-    message.transform = tf2::toMsg(odom_transform_in_map);
+    message.transform = tf2::toMsg(*last_known_odom_transform_in_map_);
     tf_broadcaster_->sendTransform(message);
   }
 }
@@ -453,6 +455,7 @@ void NdtAmclNode::initial_pose_callback(geometry_msgs::msg::PoseWithCovarianceSt
   tf2::covarianceRowMajorToEigen(message->pose.covariance, covariance);
 
   last_known_estimate_ = std::make_pair(pose, covariance);
+  last_known_odom_transform_in_map_.reset();
   initialize_from_estimate(last_known_estimate_.value());
 }