From 810b4518fe00b6df65b53fe1df1c5d35848078dc Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Sun, 19 May 2024 18:46:15 -0300 Subject: [PATCH] Fix tf broadcasting Signed-off-by: Michel Hidalgo --- beluga_amcl/include/beluga_amcl/amcl_node.hpp | 2 ++ beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp | 2 ++ beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp | 2 ++ beluga_amcl/src/amcl_node.cpp | 7 +++++-- beluga_amcl/src/amcl_nodelet.cpp | 8 ++++++-- beluga_amcl/src/ndt_amcl_node.cpp | 7 +++++-- 6 files changed, 22 insertions(+), 6 deletions(-) diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index d9d6687e3..04f82a1ed 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -168,6 +168,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode { std::unique_ptr particle_filter_; /// Last known pose estimate, if any. std::optional> last_known_estimate_; + /// Last known map to odom correction estimate, if any. + std::optional last_known_odom_transform_in_map_; /// Whether to broadcast transforms or not. bool enable_tf_broadcast_{false}; }; diff --git a/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp b/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp index 49efb1117..98d209f73 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_nodelet.hpp @@ -196,6 +196,8 @@ class AmclNodelet : public nodelet::Nodelet { std::unique_ptr particle_filter_; /// Last known pose estimate, if any. std::optional> last_known_estimate_; + /// Last known map to odom correction estimate, if any. + std::optional last_known_odom_transform_in_map_; /// Last known occupancy grid map. nav_msgs::OccupancyGrid::ConstPtr last_known_map_; /// Whether to broadcast transforms or not. diff --git a/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp index b182ff1c0..c42194d41 100644 --- a/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/ndt_amcl_node.hpp @@ -184,6 +184,8 @@ class NdtAmclNode : public rclcpp_lifecycle::LifecycleNode { std::unique_ptr particle_filter_ = nullptr; /// Last known pose estimate, if any. std::optional> last_known_estimate_ = std::nullopt; + /// Last known map to odom correction estimate, if any. + std::optional last_known_odom_transform_in_map_; /// Whether to broadcast transforms or not. bool enable_tf_broadcast_{false}; }; diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 4a2af980a..5118de9a3 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/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,7 +539,6 @@ 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) + @@ -544,7 +546,7 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ 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()); } diff --git a/beluga_amcl/src/amcl_nodelet.cpp b/beluga_amcl/src/amcl_nodelet.cpp index f736e3027..00ddc92e2 100644 --- a/beluga_amcl/src/amcl_nodelet.cpp +++ b/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(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()); } diff --git a/beluga_amcl/src/ndt_amcl_node.cpp b/beluga_amcl/src/ndt_amcl_node.cpp index 9adc28a53..f82d506ed 100644 --- a/beluga_amcl/src/ndt_amcl_node.cpp +++ b/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,7 +427,6 @@ 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) + @@ -432,7 +434,7 @@ void NdtAmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr las 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()); }