rtabmap_demos tests and docs (#1462)

* rtabmap_demos tests and docs

* added bag testing

* added rtabmap_examples launch tests

* updating demo bag download paths

* added netherdrone demo

* Fixed rgb-only callback with lidar rejected.  Updated lidar params

* Added back OrbitOriented rviz view to ros2, with optional octomap wll clipping

* fixing ci

* lidar demo added intermediate_nodes option

* added netherdrone as demo test

* running rtabmap_demos tests on ci

* added rtabmap_launch tests, fixed ground_truth_base_frame_id usage

* ficing rolling

* updating demo test harnest

* densify golden trajectories to avoid tf missing

* fixing tf steps

* updated min icp ratio for netherdrone demo

* fixing image_transport arg->params

* export pose opt=0

* lets process all frames

* updated netherdrone golden

* fixing publishers queue size just for tests

* added playdback demo doc

* Adding more logs to debug ci

* Fixing QOS for CI to reliable, added find-object demo test

* name threads

* fixing camera info expected transient on lyrical/rolling. Fixing find_object not appearing idle

* 30 Hz polling backward comp

* fixing test tf sim lock

* fixing lyrical qos bag parsing

* faster replay

* lockstep

* fixing clock deadlock

* Added test on shutdown

* updated netherdrone golden poses

* Extended stereo outdoor test

* updated shutdown test

* updated test

* multi-thread flaky test

* adding backtrace when test fails

* increased closure slack for netherdrone

* g2o gauss newton on stereo

* adjusted maximum optimizer iterations

* updated default iterations

* Added netherdrone in list of demos

rtabmap_slam/CMakeLists.txt

@@ -277,6 +277,12 @@ if(BUILD_TESTING)
   rtabmap_slam_add_node_test(test_core_wrapper_services)
   rtabmap_slam_add_node_test(test_core_wrapper_planning)
   rtabmap_slam_add_node_test(test_core_wrapper_inputs)
+
+  # The `rtabmap` executable under `ros2 launch`, stopped as Ctrl-C does.
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_pytest_test(test_shutdown test/test_shutdown.py
+    ENV ROS_DOMAIN_ID=${rtabmap_slam_test_domain_id} "PYTEST_ADDOPTS=-p no:cacheprovider"
+    TIMEOUT 300)
 endif()
 
 ament_package()

rtabmap_slam/doc/rtabmap.md

@@ -485,7 +485,7 @@ The ones that set how often a node is added, explained in [Update rate and dropp
 | `tf_tolerance` | `double` | `0.1` | How far in the future the transform is stamped, in seconds, so that lookups at the latest sensor stamp do not have to wait for it. |
 | `wait_for_transform` | `double` | `0.2` | Seconds to wait for a TF lookup before giving up on it. |
 | `ground_truth_frame_id` | `string` | `""` | The fixed frame of a ground truth system, for example `world` published by an external localization system like Vicon or OptiTrack. `ground_truth_frame_id` → `ground_truth_base_frame_id` is looked up and stored with each node, for evaluating a trajectory afterwards. |
-| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame in the ground truth tree, for example `base_link_gt`. To avoid breaking the TF tree, it represents the same frame as `frame_id`, but in a parallel TF tree, so that the robot frame does not get two parents. |
+| `ground_truth_base_frame_id` | `string` | value of `frame_id` + `"_gt"` | The robot frame in the ground truth tree, for example `base_link_gt`. Empty: the ground truth is not used. To avoid breaking the TF tree, it represents the same frame as `frame_id`, but in a parallel TF tree, so that the robot frame does not get two parents. |
 
 **This node publishes exactly one transform: `map` → `odom`.** It is the correction that puts the odometry frame where the optimized graph says it belongs — the identity until a loop closure moves it. Odometry keeps publishing `odom` → `base_link`, and the sensors must be attached to `base_link` in TF, as in the [TF tree](../README.md#frames-and-tf).
 

rtabmap_slam/package.xml

@@ -37,6 +37,13 @@
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>rtabmap_conversions</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
+  <test_depend>launch</test_depend>
+  <test_depend>launch_ros</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>rclpy</test_depend>
+  <test_depend>ros2launch</test_depend>
+  <test_depend>tf2_msgs</test_depend>
 
   <export>
 	<build_type>ament_cmake</build_type>

rtabmap_slam/src/CoreNode.cpp

@@ -99,6 +99,11 @@ int main(int argc, char** argv)
 	executor.add_node(node);
 	UINFO("rtabmap %s started...", RTABMAP_VERSION);
 	executor.spin();
+	// Close the database before rclcpp::shutdown(), which uninstalls the signal
+	// handlers: a second SIGINT (e.g., Ctrl-C then ros2 launch's own) would otherwise
+	// kill the node while it is still saving.
+	executor.remove_node(node);
+	node.reset();
 	rclcpp::shutdown();
 	return 0;
 }

rtabmap_slam/src/CoreWrapper.cpp

@@ -176,7 +176,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	odomFrameIdInit = this->declare_parameter("odom_frame_id_init", odomFrameIdInit); // set to publish map->odom TF before receiving odom topic
 	mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
 	groundTruthFrameId_ = this->declare_parameter("ground_truth_frame_id", groundTruthFrameId_);
-	groundTruthBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", frameId_);
+	groundTruthBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", frameId_+"_gt");
 
 	if(!odomFrameIdInit.empty())
 	{
@@ -2131,7 +2131,7 @@ void CoreWrapper::process(
 
 					SensorData interData(cv::Mat(), cv::Mat(), rtabmap::CameraModel(), -1, rtabmap_conversions::timestampFromROS(iter->first.header.stamp));
 					Transform gt;
-					if(!groundTruthFrameId_.empty())
+					if(!groundTruthFrameId_.empty() && !groundTruthBaseFrameId_.empty())
 					{
 						gt = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->first.header.stamp, *tfBuffer_, waitForTransform_);
 					}
@@ -2185,7 +2185,7 @@ void CoreWrapper::process(
 
 		//Add async stuff
 		Transform groundTruthPose;
-		if(!groundTruthFrameId_.empty())
+		if(!groundTruthFrameId_.empty() && !groundTruthBaseFrameId_.empty())
 		{
 			groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp, *tfBuffer_, waitForTransform_);
 		}

rtabmap_slam/test/node_test_utils.hpp

@@ -32,6 +32,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <execinfo.h>
+#include <unistd.h>
+
+#include <csignal>
+
 #include <chrono>
 #include <functional>
 #include <memory>
@@ -66,11 +71,38 @@ public:
 	}
 };
 
+/**
+ * @brief Prints the stack of a test binary that crashes, then crashes as it would have.
+ *
+ * A crash in CI shows only the return code otherwise (-11 for SIGSEGV), there being no
+ * debugger or core dump there. Library frames come with their function names; this
+ * binary's with an offset, for `addr2line -C -f -e <binary> <offset>`.
+ */
+inline void printStackOnCrash(int signal)
+{
+	void * frames[64];
+	const int count = ::backtrace(frames, 64);
+	const char header[] = "\n*** Crashed, stack trace:\n";
+	if(::write(STDERR_FILENO, header, sizeof(header) - 1) < 0) {}
+	::backtrace_symbols_fd(frames, count, STDERR_FILENO);
+	std::signal(signal, SIG_DFL);
+	std::raise(signal);
+}
+
 /// Registers RclcppEnvironment. Call once at file scope in each test binary.
 inline ::testing::Environment * registerRclcppEnvironment()
 {
-	static ::testing::Environment * const env =
-			::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
+	static ::testing::Environment * const env = []() {
+		// backtrace() loads libgcc on its first call, which is not safe in a signal
+		// handler: done here.
+		void * frame;
+		::backtrace(&frame, 1);
+		for(int signal : {SIGSEGV, SIGBUS, SIGFPE, SIGILL, SIGABRT})
+		{
+			std::signal(signal, printStackOnCrash);
+		}
+		return ::testing::AddGlobalTestEnvironment(new RclcppEnvironment);
+	}();
 	return env;
 }
 

rtabmap_slam/test/rtabmap_odom_only.launch.py

@@ -0,0 +1,22 @@
+# rtabmap alone, mapping from odometry only, for test_shutdown.py.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('database_path'),
+        DeclareLaunchArgument('working_directory'),
+        Node(
+            package='rtabmap_slam', executable='rtabmap', output='screen',
+            parameters=[{
+                'database_path': LaunchConfiguration('database_path'),
+                'Rtabmap/WorkingDirectory': LaunchConfiguration('working_directory'),
+                'subscribe_depth': False,
+                'subscribe_rgb': False,
+                'Rtabmap/DetectionRate': '0'}],
+            arguments=['-d']),
+    ])

rtabmap_slam/test/test_core_wrapper_inputs.cpp

@@ -68,6 +68,45 @@ protected:
 		return nullptr;
 	}
 
+	/**
+	 * @brief Calls @p send until the node processes the update stamped @p stamp: an info
+	 *        at that stamp.
+	 *
+	 * For a node on its own multi-threaded executor (makeMultiThreadedNode()): it
+	 * publishes info from inside its update, and takes new data only once that update
+	 * has returned, dropping what arrives in between. Woken by info, the test can send
+	 * the next update in that window, on a slow machine; it is then sent again. A copy
+	 * sent while the first is still being processed is dropped the same way; one that
+	 * was processed anyway only adds an info at a stamp already seen. A node that never
+	 * takes new data again still fails.
+	 */
+	bool sendUntilProcessed(
+			const std::function<void()> & send,
+			const std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> & info,
+			double stamp)
+	{
+		const rclcpp::Time expected(stampOf(stamp));
+		auto processed = [&]() {
+			for(const rtabmap_msgs::msg::Info::ConstSharedPtr & msg : info->messages)
+			{
+				if(rclcpp::Time(msg->header.stamp) == expected)
+				{
+					return true;
+				}
+			}
+			return false;
+		};
+		for(int attempt=0; attempt<10; ++attempt)
+		{
+			send();
+			if(spinUntil(processed, std::chrono::milliseconds(500)))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
 	static bool hasPose(const rtabmap_msgs::msg::MapGraph & graph, int id)
 	{
 		return std::find(graph.poses_id.begin(), graph.poses_id.end(), id) != graph.poses_id.end();
@@ -304,11 +343,10 @@ TEST_F(CoreWrapperInputsTest, maps_the_next_scans_after_one_without_its_tf)
 	publishStaticTf("laser");
 	for(int i=1; i<=3; ++i)
 	{
-		const size_t before = info->size();
-		sendOdom(odom, 1.0 + i, 0.5*i);
-		scan->publish(makeRoomScan("laser", 1.0 + i, 0.5*i));
-		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
-				<< "scan " << i << " was not processed after the one without TF";
+		ASSERT_TRUE(sendUntilProcessed([&]() {
+				sendOdom(odom, 1.0 + i, 0.5*i);
+				scan->publish(makeRoomScan("laser", 1.0 + i, 0.5*i));
+			}, info, 1.0 + i)) << "scan " << i << " was not processed after the one without TF";
 	}
 	EXPECT_EQ(3u, getGraph().graph.poses_id.size());
 }
@@ -332,11 +370,10 @@ TEST_F(CoreWrapperInputsTest, maps_the_next_clouds_after_one_without_its_tf)
 	publishStaticTf("lidar", 0.0, 0.0, 0.5);
 	for(int i=1; i<=3; ++i)
 	{
-		const size_t before = info->size();
-		sendOdom(odom, 1.0 + i, 0.5*i);
-		cloud->publish(makeCloud("lidar", 1.0 + i, roomScan3d(0.5*i, 0.0, 0.5)));
-		ASSERT_TRUE(spinUntil([&]() { return info->size() > before; }))
-				<< "cloud " << i << " was not processed after the one without TF";
+		ASSERT_TRUE(sendUntilProcessed([&]() {
+				sendOdom(odom, 1.0 + i, 0.5*i);
+				cloud->publish(makeCloud("lidar", 1.0 + i, roomScan3d(0.5*i, 0.0, 0.5)));
+			}, info, 1.0 + i)) << "cloud " << i << " was not processed after the one without TF";
 	}
 	EXPECT_EQ(3u, getGraph().graph.poses_id.size());
 }
@@ -354,7 +391,10 @@ TEST_F(CoreWrapperInputsTest, reads_odometry_from_tf_with_odom_frame_id)
 	rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan =
 			helper()->create_publisher<sensor_msgs::msg::LaserScan>("scan", 10);
 	ASSERT_TRUE(waitForSubscriber(scan));
-	EXPECT_EQ(0u, helper()->count_publishers("odom") + helper()->count_subscribers("odom"));
+	// Waits: the graph can still list the previous test's odom endpoints for a moment.
+	EXPECT_TRUE(spinUntil([&]() {
+		return helper()->count_publishers("odom") + helper()->count_subscribers("odom") == 0; }))
+			<< "something subscribes or publishes on odom";
 
 	driveWithScans(nullptr, scan, info, 3, 0.0);
 

rtabmap_slam/test/test_core_wrapper_mapping.cpp

@@ -93,6 +93,48 @@ TEST_F(CoreWrapperMappingTest, adds_a_node_per_odometry_update)
 	EXPECT_NE(0, neighborLink(map.graph, 2, 3).from_id);
 }
 
+/**
+ * With a ground truth frame, each node gets the ground truth's pose at its stamp, and
+ * rtabmap reports its error against it (Gt/* statistics): ground_truth_frame_id ->
+ * ground_truth_base_frame_id, which defaults to frame_id + "_gt". Here only base_link_gt
+ * is in TF, following the odometry exactly. Empty, ground_truth_base_frame_id turns the
+ * ground truth off.
+ */
+class CoreWrapperGroundTruthTest : public CoreWrapperMappingTest
+{
+protected:
+	/// Gt/Translational_rmse/m after three updates with @p params, -1 if not reported.
+	float groundTruthRmse(const std::vector<rclcpp::Parameter> & params)
+	{
+		makeNode(params);
+		std::shared_ptr<Collector<rtabmap_msgs::msg::Info>> info = collectInfo();
+		rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom = odomPublisher();
+		for(int i=0; i<3; ++i)
+		{
+			const size_t before = info->size();
+			publishTf(makeTransform("world", "base_link_gt", 1.0 + i, 0.5 * i));
+			sendOdom(odom, 1.0 + i, 0.5 * i);
+			EXPECT_TRUE(spinUntil([&]() { return info->size() > before; }))
+					<< "update " << i << " was not processed";
+		}
+		return info->empty() ? -1.0f : stat(info->back(), "Gt/Translational_rmse/m");
+	}
+};
+
+TEST_F(CoreWrapperGroundTruthTest, compares_with_the_ground_truth_of_frame_id_gt_by_default)
+{
+	const float rmse = groundTruthRmse({rclcpp::Parameter("ground_truth_frame_id", "world")});
+	ASSERT_GE(rmse, 0.0f) << "no Gt/* statistics: the ground truth was not found";
+	EXPECT_NEAR(0.0f, rmse, 1e-3);
+}
+
+TEST_F(CoreWrapperGroundTruthTest, ignores_the_ground_truth_when_its_base_frame_is_empty)
+{
+	EXPECT_LT(groundTruthRmse({rclcpp::Parameter("ground_truth_frame_id", "world"),
+	                           rclcpp::Parameter("ground_truth_base_frame_id", "")}), 0.0f)
+			<< "Gt/* statistics reported although the ground truth is off";
+}
+
 /**
  * An update that did not move at least RGBD/LinearUpdate (or turn RGBD/AngularUpdate)
  * since the last node is not added: a robot standing still does not grow the map.

rtabmap_slam/test/test_core_wrapper_planning.cpp

@@ -50,7 +50,14 @@ protected:
 		ASSERT_TRUE(waitForPublisher(goalReached_->subscription));
 		ASSERT_TRUE(waitForPublisher(globalPath_->subscription));
 		ASSERT_TRUE(waitForPublisher(globalPathNodes_->subscription));
-		driveStraight(odom_, info_, 5);   // nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0
+		// Nodes 1..5 at x = 0, 0.5, 1.0, 1.5, 2.0.
+		for(int i=0; i<5; ++i)
+		{
+			const size_t before = info_->size();
+			sendUpdate(1.0 + i, 0.5 * i);
+			ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }))
+					<< "update " << i << " was not processed";
+		}
 		nextStamp_ = 6.0;
 	}
 
@@ -65,11 +72,17 @@ protected:
 
 	virtual std::vector<rclcpp::Parameter> nodeParameters() { return {}; }
 
+	/// Sends one odometry update stamped @p stamp, the robot at (@p x, 0, @p yaw).
+	virtual void sendUpdate(double stamp, double x, double yaw = 0.0)
+	{
+		sendOdom(odom_, stamp, x, 0.0, yaw);
+	}
+
 	/// Moves the robot to @p x and waits for the update to be processed.
 	bool moveTo(double x)
 	{
 		const size_t before = info_->size();
-		sendOdom(odom_, nextStamp_, x);
+		sendUpdate(nextStamp_, x);
 		nextStamp_ += 1.0;
 		return spinUntil([&]() { return info_->size() > before; });
 	}
@@ -244,9 +257,13 @@ TEST_F(CoreWrapperPlanningTest, plans_through_the_graph_beyond_the_local_radius)
 /**
  * The same corridor, with the node on the tests' own clock (use_sim_time): map -> odom is
  * then stamped in the odometry's time base, and with tf_tolerance at 0, exactly at the
- * clock's time. Only for tests whose TF lookups never have to wait: with a clock that only
- * moves when told to, a lookup waiting for a transform that is not there -- a goal in an
- * unknown frame, say -- would wait forever.
+ * clock's time. The clock is set to each update's stamp before it is sent. A TF lookup
+ * waits on that clock (tf2_ros sleeps on it since Rolling/Lyrical): frozen, a lookup that
+ * has to wait never returns, even once its transform is there. Each update's transform is
+ * then sent first, the clock and some time for the node's listener after it, the odometry
+ * last, so that its lookup never has to wait. Only for tests whose lookups find their
+ * transform: one for a transform that is not there -- a goal in an unknown frame, say --
+ * would wait forever.
  */
 class CoreWrapperPlanningSimTimeTest : public CoreWrapperPlanningTest
 {
@@ -257,6 +274,13 @@ protected:
 				rclcpp::Parameter("tf_tolerance", 0.0)};
 	}
 
+	void sendUpdate(double stamp, double x, double yaw = 0.0) override
+	{
+		publishTf(makeTransform("odom", "base_link", stamp, x, 0.0, yaw));
+		setClock(stamp);
+		odom_->publish(makeOdometry(stamp, x, 0.0, yaw));
+	}
+
 	/// Sets the node's clock to @p seconds.
 	void setClock(double seconds)
 	{
@@ -278,7 +302,7 @@ TEST_F(CoreWrapperPlanningSimTimeTest, transforms_a_goal_in_the_robot_frame_to_t
 {
 	// Turn the robot to face +y where it stands, at x = 2.
 	const size_t before = info_->size();
-	sendOdom(odom_, nextStamp_, 2.0, 0.0, M_PI/2.0);
+	sendUpdate(nextStamp_, 2.0, M_PI/2.0);
 	ASSERT_TRUE(spinUntil([&]() { return info_->size() > before; }));
 
 	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr goal =

rtabmap_slam/test/test_shutdown.py

@@ -0,0 +1,112 @@
+"""
+Ctrl-C on `ros2 launch` leaves rtabmap's database complete.
+
+Ctrl-C sends SIGINT to the whole process group, and `ros2 launch` then sends its own
+SIGINT to each node: rtabmap gets two. It saves what it kept in memory only -- the
+optimized graph, the links added to nodes already in the database -- when it closes the
+database, so it has to survive the second SIGINT until then.
+"""
+
+import os
+import signal
+import sqlite3
+import subprocess
+import time
+from pathlib import Path
+
+import rclpy
+from geometry_msgs.msg import TransformStamped
+from nav_msgs.msg import Odometry
+from rclpy.qos import QoSProfile
+from rtabmap_msgs.msg import Info
+from tf2_msgs.msg import TFMessage
+
+LAUNCH_FILE = Path(__file__).parent / 'rtabmap_odom_only.launch.py'
+NODES = 5
+
+
+def _odometry(stamp: float, x: float):
+    """odom -> base_link at @p x, as TF and as an odometry message, the way the C++
+    tests' sendOdom() sends them."""
+    tf = TransformStamped()
+    tf.header.frame_id = 'odom'
+    tf.header.stamp.sec = int(stamp)
+    tf.child_frame_id = 'base_link'
+    tf.transform.translation.x = x
+    tf.transform.rotation.w = 1.0
+    odom = Odometry()
+    odom.header = tf.header
+    odom.child_frame_id = 'base_link'
+    odom.pose.pose.position.x = x
+    odom.pose.pose.orientation.w = 1.0
+    odom.pose.covariance = [0.001 if i % 7 == 0 else 0.0 for i in range(36)]
+    odom.twist.covariance = odom.pose.covariance
+    return TFMessage(transforms=[tf]), odom
+
+
+def _spin_until(node, done, timeout: float) -> bool:
+    end = time.monotonic() + timeout
+    while time.monotonic() < end:
+        if done():
+            return True
+        rclpy.spin_once(node, timeout_sec=0.05)
+    return done()
+
+
+def _map(node, launch):
+    """Drives NODES odometry updates into rtabmap, waiting for each to be processed."""
+    tf_pub = node.create_publisher(TFMessage, '/tf', QoSProfile(depth=100))
+    odom_pub = node.create_publisher(Odometry, 'odom', 10)
+    info = []
+    node.create_subscription(Info, 'info', info.append, 10)
+    started = lambda: (launch.poll() is not None or (  # noqa: E731
+        odom_pub.get_subscription_count() > 0 and tf_pub.get_subscription_count() > 0
+        and node.count_publishers('info') > 0))
+    assert _spin_until(node, started, 60.0) and launch.poll() is None, 'rtabmap did not start'
+    # Matched on this side only: rtabmap's side matches on its own time, and until then
+    # its best-effort subscription (Fast DDS's default) drops what it is sent, and it
+    # does not publish info, having no subscriber.
+    _spin_until(node, lambda: False, 1.0)
+    for i in range(NODES):
+        tf, odom = _odometry(1.0 + i, 0.5 * i)
+        tf_pub.publish(tf)
+        odom_pub.publish(odom)
+        assert _spin_until(node, lambda: len(info) > i, 10.0), f'update {i} was not processed'
+
+
+def test_ctrl_c_saves_the_database(tmp_path):
+    database = tmp_path / 'rtabmap.db'
+    log_path = tmp_path / 'launch.log'
+    with open(log_path, 'w') as log:
+        launch = subprocess.Popen(
+            ['ros2', 'launch', str(LAUNCH_FILE), f'database_path:={database}',
+             f'working_directory:={tmp_path}'],
+            stdout=log, stderr=subprocess.STDOUT, start_new_session=True)
+    rclpy.init()
+    try:
+        node = rclpy.create_node('rtabmap_slam_test_shutdown')
+        try:
+            _map(node, launch)
+        finally:
+            node.destroy_node()
+        os.killpg(launch.pid, signal.SIGINT)  # Ctrl-C
+        launch.wait(timeout=60)
+    except AssertionError as e:
+        raise AssertionError(f'{e}\n{log_path.read_text()}') from None
+    finally:
+        rclpy.try_shutdown()
+        if launch.poll() is None:
+            os.killpg(launch.pid, signal.SIGKILL)
+            launch.wait()
+    log = log_path.read_text()
+
+    assert 'process has finished cleanly' in log, log
+    with sqlite3.connect(database) as db:
+        optimized = db.execute('SELECT opt_poses FROM Admin').fetchone()[0]
+        # Each neighbor link is in both directions; new -> old goes in with the new node,
+        # old -> new is added to a node already saved, and is written at closing.
+        forward, backward = (
+            db.execute(f'SELECT COUNT(*) FROM Link WHERE type = 0 AND from_id {op} to_id')
+            .fetchone()[0] for op in ('<', '>'))
+    assert optimized, 'no optimized graph saved'
+    assert (forward, backward) == (NODES - 1, NODES - 1)