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 a24859bb4a..b7d58b6736 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 */ @@ -125,6 +128,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 f0fb4ef5dd..1b0c36529e 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -7,6 +7,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 313d71fb0a..939555bf3d 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 df4e86b63e..d4d2ea1adf 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -30,10 +30,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()); @@ -75,11 +76,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 (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, 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 not dependent on state estimation + // frames. + 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 3ef51e2b69..7c12a4bd52 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -30,10 +30,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()); @@ -84,11 +85,29 @@ 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 (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, 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 not dependent on state estimation + // frames. + 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 d1f52b31f1..50f670cb13 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -27,10 +27,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()); @@ -73,11 +74,29 @@ 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 (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, 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 not dependent on state estimation + // frames. + 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 bd1028518c..2b73672368 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -33,10 +33,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