rtabmap_slam tests and doc (#1460)

* rtabmap_slam tests and doc * another round of review of the doc * Disable by default use_intra_process_comms on latched/transient publishers * Added test to catch not unlocked mutex from early error exit * updated coverage settings * Using UScopeMutex on all tryLock()
2026-10-04 16:57:46 +08:00 · 2026-09-28 13:09:51 -07:00
parent 853a434fe9
commit 5207dab7c2
30 changed files with 5832 additions and 118 deletions
@@ -0,0 +1,256 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef RTABMAP_SLAM_NODE_TEST_UTILS_HPP_
+#define RTABMAP_SLAM_NODE_TEST_UTILS_HPP_
+
+#include <gtest/gtest.h>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace rtabmap_slam_test {
+
+/**
+ * @brief Brings rclcpp up once for the whole test binary.
+ *
+ * Registered as a gtest global environment so it runs before the first test and shuts
+ * down after the last one, which keeps gtest_main usable.
+ */
+class RclcppEnvironment : public ::testing::Environment
+{
+public:
+	void SetUp() override
+	{
+		if(!rclcpp::ok())
+		{
+			rclcpp::init(0, nullptr);
+		}
+	}
+	void TearDown() override
+	{
+		if(rclcpp::ok())
+		{
+			rclcpp::shutdown();
+		}
+	}
+};
+
+/// Registers RclcppEnvironment. Call once at file scope in each test binary.
+inline ::testing::Environment * registerRclcppEnvironment()
+{
+	static ::testing::Environment * const env =
+			::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
+	return env;
+}
+
+/**
+ * @brief Base fixture for driving a node under test over real ROS topics.
+ *
+ * The node under test and a helper node share one single-threaded executor, so
+ * publishing, the node's callback and the assertion all happen on the same thread and
+ * the tests stay deterministic. No launch files and no separate processes are involved:
+ * everything runs in the gtest binary.
+ */
+class NodeTest : public ::testing::Test
+{
+protected:
+	void SetUp() override
+	{
+		executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+		helper_ = std::make_shared<rclcpp::Node>("rtabmap_slam_test_helper");
+		executor_->add_node(helper_);
+	}
+
+	void TearDown() override
+	{
+		for(const rclcpp::Node::SharedPtr & node : nodes_)
+		{
+			executor_->remove_node(node);
+		}
+		nodes_.clear();
+		executor_->remove_node(helper_);
+		helper_.reset();
+		executor_.reset();
+	}
+
+	/**
+	 * @brief Adds a node under test to the shared executor and keeps it alive for the test.
+	 *
+	 * The wait is for tf2_ros, not for anything the test does with the node.
+	 * ~TransformListener cancels its worker's executor and joins it without ordering the
+	 * cancel after the worker reached spin(), so a node dropped microseconds after it was
+	 * built -- which a test that only reads a parameter back does -- hangs the binary for
+	 * good (ros2/geometry2#517). The window is a few instructions wide and nothing here
+	 * can observe that thread, so this buys time instead. Drop it once #752 lands.
+	 */
+	template <typename NodeT>
+	std::shared_ptr<NodeT> addNode(const std::shared_ptr<NodeT> & node)
+	{
+		executor_->add_node(node);
+		nodes_.push_back(node);
+		spinFor(std::chrono::milliseconds(50));
+		return node;
+	}
+
+	/// The helper node, used to publish inputs and subscribe to outputs.
+	rclcpp::Node::SharedPtr helper() { return helper_; }
+
+	/**
+	 * @brief Spins until @p done returns true, or the timeout elapses.
+	 * @return true if @p done became true
+	 */
+	bool spinUntil(
+			const std::function<bool()> & done,
+			std::chrono::milliseconds timeout = std::chrono::milliseconds(5000))
+	{
+		const std::chrono::steady_clock::time_point deadline =
+				std::chrono::steady_clock::now() + timeout;
+		while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
+		{
+			if(done())
+			{
+				return true;
+			}
+			executor_->spin_once(std::chrono::milliseconds(10));
+		}
+		return done();
+	}
+
+	/// Spins for a fixed duration, for the "nothing should happen" assertions.
+	void spinFor(std::chrono::milliseconds duration)
+	{
+		const std::chrono::steady_clock::time_point deadline =
+				std::chrono::steady_clock::now() + duration;
+		while(rclcpp::ok() && std::chrono::steady_clock::now() < deadline)
+		{
+			executor_->spin_once(std::chrono::milliseconds(10));
+		}
+	}
+
+	/**
+	 * @brief Waits until @p publisher has at least @p count matched subscriptions.
+	 *
+	 * Publishing before the node under test has discovered the topic silently drops the
+	 * message, which is the most common cause of a flaky in-process node test.
+	 */
+	template <typename PublisherT>
+	bool waitForSubscriber(const PublisherT & publisher, size_t count = 1)
+	{
+		return spinUntil([&]() { return publisher->get_subscription_count() >= count; });
+	}
+
+	/**
+	 * @brief Waits until @p subscription sees at least one publisher.
+	 *
+	 * Every node here publishes only when it has subscribers, so the test's subscription
+	 * has to be discovered before the input is sent.
+	 */
+	template <typename SubscriptionT>
+	bool waitForPublisher(const SubscriptionT & subscription, size_t count = 1)
+	{
+		return spinUntil([&]() { return subscription->get_publisher_count() >= count; });
+	}
+
+	/// Collects every message received on @p topic, for later assertions.
+	template <typename MsgT>
+	struct Collector
+	{
+		typename rclcpp::Subscription<MsgT>::SharedPtr subscription;
+		std::vector<typename MsgT::ConstSharedPtr> messages;
+		size_t size() const { return messages.size(); }
+		bool empty() const { return messages.empty(); }
+
+		/**
+		 * @brief The last (first) message received.
+		 *
+		 * A test that reads these without having waited for the topic it is reading --
+		 * having waited for a different one, say -- gets a legible failure rather than a
+		 * segmentation fault: std::vector::back() on an empty vector dereferences
+		 * nullptr-1, which crashes the whole binary and takes the rest of its tests with
+		 * it. gtest turns the exception into a failure of the test that threw it.
+		 */
+		const MsgT & back() const { return *checked(messages.empty()?0:&messages.back()); }
+		const MsgT & front() const { return *checked(messages.empty()?0:&messages.front()); }
+
+	private:
+		const typename MsgT::ConstSharedPtr & checked(
+				const typename MsgT::ConstSharedPtr * msg) const
+		{
+			if(msg == 0)
+			{
+				throw std::out_of_range(
+						std::string("nothing was received on \"") +
+						(subscription?subscription->get_topic_name():"?") +
+						"\", so there is no message to read: wait for it to arrive first");
+			}
+			return *msg;
+		}
+	};
+
+	/**
+	 * @brief Subscribes the helper node to @p topic and records everything it receives.
+	 *
+	 * The callback holds the collector weakly. Capturing it by shared_ptr would close a
+	 * cycle -- collector owns the subscription, the subscription owns the callback, the
+	 * callback owns the collector -- and neither would ever be freed. A subscription that
+	 * outlives its test keeps the helper node's rcl handle alive with it, which leaves the
+	 * node's rosout publisher registered and greets the next test with "Publisher already
+	 * registered for node name: 'rtabmap_slam_test_helper'".
+	 */
+	template <typename MsgT>
+	std::shared_ptr<Collector<MsgT>> collect(
+			const std::string & topic, const rclcpp::QoS & qos = rclcpp::QoS(10))
+	{
+		std::shared_ptr<Collector<MsgT>> collector = std::make_shared<Collector<MsgT>>();
+		std::weak_ptr<Collector<MsgT>> weak = collector;
+		collector->subscription = helper_->create_subscription<MsgT>(
+				topic, qos,
+				[weak](const typename MsgT::ConstSharedPtr msg) {
+					if(std::shared_ptr<Collector<MsgT>> collector = weak.lock())
+					{
+						collector->messages.push_back(msg);
+					}
+				});
+		return collector;
+	}
+
+private:
+	rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+	rclcpp::Node::SharedPtr helper_;
+	std::vector<rclcpp::Node::SharedPtr> nodes_;
+};
+
+}  // namespace rtabmap_slam_test
+
+#endif /* RTABMAP_SLAM_NODE_TEST_UTILS_HPP_ */