diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 32227e1df39..35488e082ca 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -100,7 +100,6 @@ class Source bool getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, tf2::Transform & tf_transform) const; // ----- Variables ----- diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index ed39ade7a2f..ede48846c4f 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -111,7 +111,7 @@ void PointCloud::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if (!getTransform(data_->header.frame_id, data_->header.stamp, tf_transform)) { return; } diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index a73d9e53cf0..66ef3b068c4 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -76,7 +76,7 @@ void Scan::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if (!getTransform(data_->header.frame_id, data_->header.stamp, tf_transform)) { return; } diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index e9453016406..dd562ee1b32 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -79,7 +79,6 @@ bool Source::sourceValid( bool Source::getTransform( const std::string & source_frame_id, const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, tf2::Transform & tf2_transform) const { geometry_msgs::msg::TransformStamped transform; @@ -88,10 +87,7 @@ bool Source::getTransform( try { // Obtaining the transform to get data from source to base frame. // This also considers the time shift between source and base. - transform = tf_buffer_->lookupTransform( - base_frame_id_, curr_time, - source_frame_id, source_time, - global_frame_id_, transform_tolerance_); + transform = tf_buffer_->lookupTransform(base_frame_id_, source_frame_id, source_time, transform_tolerance_); } catch (tf2::TransformException & e) { RCLCPP_ERROR( logger_,