From 88646b6e1804478f7e37cdbe1fbab60ec951bc91 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 16 Feb 2023 11:57:33 +0300 Subject: [PATCH 1/2] Option allowing to use simple lookupTransform API ignoring time shifts between source and base frame during the movement --- .../collision_monitor_node.hpp | 5 +- .../nav2_collision_monitor/pointcloud.hpp | 5 +- .../include/nav2_collision_monitor/range.hpp | 5 +- .../include/nav2_collision_monitor/scan.hpp | 5 +- .../include/nav2_collision_monitor/source.hpp | 8 +- .../params/collision_monitor_params.yaml | 1 + .../src/collision_monitor_node.cpp | 18 +++- nav2_collision_monitor/src/pointcloud.cpp | 36 ++++--- nav2_collision_monitor/src/range.cpp | 33 +++++-- nav2_collision_monitor/src/scan.cpp | 33 +++++-- nav2_collision_monitor/src/source.cpp | 6 +- nav2_collision_monitor/test/sources_test.cpp | 94 ++++++++++++++----- 12 files changed, 187 insertions(+), 62 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 87f498e401..2dc2995856 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -127,13 +127,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode * source->base time inerpolated transform. * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time * @return True if all sources were configured successfully or false in failure case */ bool configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Main processing routine diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index c316c065f7..d5af32c277 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -41,6 +41,8 @@ class PointCloud : public Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ PointCloud( const nav2_util::LifecycleNode::WeakPtr & node, @@ -49,7 +51,8 @@ class PointCloud : public Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief PointCloud destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 0fbe47501a..777b8e404c 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -41,6 +41,8 @@ class Range : public Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ Range( const nav2_util::LifecycleNode::WeakPtr & node, @@ -49,7 +51,8 @@ class Range : public Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Range destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index 29747e8131..c3f7a11f3f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -41,6 +41,8 @@ class Scan : public Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ Scan( const nav2_util::LifecycleNode::WeakPtr & node, @@ -49,7 +51,8 @@ class Scan : public Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Scan destructor */ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 5fd95de7ee..8bc750cc71 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -46,6 +46,8 @@ class Source * @param global_frame_id Global frame ID for correct transform calculation * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time */ Source( const nav2_util::LifecycleNode::WeakPtr & node, @@ -54,7 +56,8 @@ class Source const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Source destructor */ @@ -110,6 +113,9 @@ class Source tf2::Duration transform_tolerance_; /// @brief Maximum time interval in which data is considered valid rclcpp::Duration source_timeout_; + /// @brief Whether to correct source data towards to base frame movement, + /// considering the difference between current time and latest source time + bool base_shift_correction_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 4e5f90ae80..9407b40b7b 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -6,6 +6,7 @@ collision_monitor: cmd_vel_out_topic: "cmd_vel" transform_tolerance: 0.5 source_timeout: 5.0 + base_shift_correction: True stop_pub_timeout: 2.0 # Polygons represent zone around the robot for "stop" and "slowdown" action types, # and robot footprint for "approach" action type. diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index d706ba0cd9..56a6e6b6a0 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -205,6 +205,10 @@ bool CollisionMonitor::getParameters( node, "source_timeout", rclcpp::ParameterValue(2.0)); source_timeout = rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "base_shift_correction", rclcpp::ParameterValue(true)); + const bool base_shift_correction = + get_parameter("base_shift_correction").as_bool(); nav2_util::declare_parameter_if_not_declared( node, "stop_pub_timeout", rclcpp::ParameterValue(1.0)); @@ -215,7 +219,10 @@ bool CollisionMonitor::getParameters( return false; } - if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) { + if ( + !configureSources( + base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction)) + { return false; } @@ -271,7 +278,8 @@ bool CollisionMonitor::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) { try { auto node = shared_from_this(); @@ -289,7 +297,7 @@ bool CollisionMonitor::configureSources( if (source_type == "scan") { std::shared_ptr s = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); s->configure(); @@ -297,7 +305,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "pointcloud") { std::shared_ptr p = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); p->configure(); @@ -305,7 +313,7 @@ bool CollisionMonitor::configureSources( } else if (source_type == "range") { std::shared_ptr r = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); r->configure(); diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 282dec5463..0ea08f28a7 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -31,10 +31,11 @@ PointCloud::PointCloud( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str()); @@ -76,16 +77,29 @@ void PointCloud::getData( return; } - // 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 ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } } sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 0ba4be75aa..66d0d887be 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -31,10 +31,11 @@ Range::Range( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str()); @@ -88,13 +89,27 @@ void Range::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 ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } } // Calculate poses and refill data array diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 63ea1d6a76..76b61971c1 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -29,10 +29,11 @@ Scan::Scan( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : Source( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, source_timeout), + transform_tolerance, source_timeout, base_shift_correction), data_(nullptr) { RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str()); @@ -78,13 +79,27 @@ 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 ( - !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, - base_frame_id_, curr_time, global_frame_id_, - transform_tolerance_, tf_buffer_, tf_transform)) - { - return; + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option. + if ( + !nav2_util::getTransform( + data_->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } } // Calculate poses and refill data array diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index a7de3bbe4e..df539495f2 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -30,10 +30,12 @@ Source::Source( const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) : node_(node), source_name_(source_name), tf_buffer_(tf_buffer), base_frame_id_(base_frame_id), global_frame_id_(global_frame_id), - transform_tolerance_(transform_tolerance), source_timeout_(source_timeout) + transform_tolerance_(transform_tolerance), source_timeout_(source_timeout), + base_shift_correction_(base_shift_correction) { } diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 7101b61175..0bfcd1a062 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -172,10 +172,11 @@ class ScanWrapper : public nav2_collision_monitor::Scan const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & data_timeout) + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) : nav2_collision_monitor::Scan( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -194,10 +195,11 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & data_timeout) + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) : nav2_collision_monitor::PointCloud( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -216,10 +218,11 @@ class RangeWrapper : public nav2_collision_monitor::Range const std::string & base_frame_id, const std::string & global_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & data_timeout) + const rclcpp::Duration & data_timeout, + const bool base_shift_correction) : nav2_collision_monitor::Range( node, source_name, tf_buffer, base_frame_id, global_frame_id, - transform_tolerance, data_timeout) + transform_tolerance, data_timeout, base_shift_correction) {} bool dataReceived() const @@ -235,6 +238,9 @@ class Tester : public ::testing::Test ~Tester(); protected: + // Data sources creation routine + void createSources(const bool base_shift_correction = true); + // Setting TF chains void sendTransforms(const rclcpp::Time & stamp); @@ -263,7 +269,22 @@ Tester::Tester() tf_buffer_ = std::make_shared(test_node_->get_clock()); tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model tf_listener_ = std::make_shared(*tf_buffer_); +} + +Tester::~Tester() +{ + scan_.reset(); + pointcloud_.reset(); + range_.reset(); + test_node_.reset(); + + tf_listener_.reset(); + tf_buffer_.reset(); +} + +void Tester::createSources(const bool base_shift_correction) +{ // Create Scan object test_node_->declare_parameter( std::string(SCAN_NAME) + ".topic", rclcpp::ParameterValue(SCAN_TOPIC)); @@ -273,7 +294,7 @@ Tester::Tester() scan_ = std::make_shared( test_node_, SCAN_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); scan_->configure(); // Create PointCloud object @@ -293,7 +314,7 @@ Tester::Tester() pointcloud_ = std::make_shared( test_node_, POINTCLOUD_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); pointcloud_->configure(); // Create Range object @@ -308,22 +329,10 @@ Tester::Tester() range_ = std::make_shared( test_node_, RANGE_NAME, tf_buffer_, BASE_FRAME_ID, GLOBAL_FRAME_ID, - TRANSFORM_TOLERANCE, DATA_TIMEOUT); + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); range_->configure(); } -Tester::~Tester() -{ - scan_.reset(); - pointcloud_.reset(); - range_.reset(); - - test_node_.reset(); - - tf_listener_.reset(); - tf_buffer_.reset(); -} - void Tester::sendTransforms(const rclcpp::Time & stamp) { std::shared_ptr tf_broadcaster = @@ -453,6 +462,8 @@ TEST_F(Tester, testGetData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + sendTransforms(curr_time); // Publish data for sources @@ -485,6 +496,8 @@ TEST_F(Tester, testGetOutdatedData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + sendTransforms(curr_time); // Publish outdated data for sources @@ -515,6 +528,8 @@ TEST_F(Tester, testIncorrectFrameData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + // Send incorrect transform sendTransforms(curr_time - 1s); @@ -546,6 +561,8 @@ TEST_F(Tester, testIncorrectData) { rclcpp::Time curr_time = test_node_->now(); + createSources(); + sendTransforms(curr_time); // Publish data for sources @@ -567,6 +584,41 @@ TEST_F(Tester, testIncorrectData) ASSERT_EQ(data.size(), 0u); } +TEST_F(Tester, testIgnoreTimeShift) +{ + rclcpp::Time curr_time = test_node_->now(); + + createSources(false); + + // Send incorrect transform + sendTransforms(curr_time - 1s); + + // Publish data for sources + test_node_->publishScan(curr_time, 1.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); + + // Scan data should be consistent + std::vector data; + scan_->getData(curr_time, data); + checkScan(data); + + // Pointcloud data should be consistent + data.clear(); + pointcloud_->getData(curr_time, data); + checkPointCloud(data); + + // Range data should be consistent + data.clear(); + range_->getData(curr_time, data); + checkRange(data); +} + int main(int argc, char ** argv) { // Initialize the system From 7e432ecf90f8982cf38f88a527406becf5e9090c Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Fri, 17 Feb 2023 11:14:30 +0300 Subject: [PATCH 2/2] Refine comments --- nav2_collision_monitor/src/pointcloud.cpp | 4 ++-- nav2_collision_monitor/src/range.cpp | 5 ++--- nav2_collision_monitor/src/scan.cpp | 5 ++--- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 0ea08f28a7..4582703b2f 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -77,7 +77,6 @@ void PointCloud::getData( return; } - tf2::Transform tf_transform; if (base_shift_correction_) { // Obtaining the transform to get data from source frame and time where it was received @@ -92,7 +91,8 @@ void PointCloud::getData( } } else { // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option. + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. if ( !nav2_util::getTransform( data_->header.frame_id, base_frame_id_, diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 66d0d887be..556e35da46 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -86,8 +86,6 @@ void Range::getData( return; } - // 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 (base_shift_correction_) { // Obtaining the transform to get data from source frame and time where it was received @@ -102,7 +100,8 @@ void Range::getData( } } else { // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option. + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. if ( !nav2_util::getTransform( data_->header.frame_id, base_frame_id_, diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 76b61971c1..731ee8baa8 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -76,8 +76,6 @@ void Scan::getData( return; } - // 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 (base_shift_correction_) { // Obtaining the transform to get data from source frame and time where it was received @@ -92,7 +90,8 @@ void Scan::getData( } } else { // Obtaining the transform to get data from source frame to base frame without time shift - // considered. Less accurate but much more faster option. + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. if ( !nav2_util::getTransform( data_->header.frame_id, base_frame_id_,