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
2026-10-12 04:29:49 +08:00 · 2026-10-10 12:23:49 -07:00
parent cb2117f1ed
commit 82f0754bf7
80 changed files with 18405 additions and 98 deletions
@@ -78,7 +78,7 @@ These apply to all three nodes.
 | `wait_for_transform` | `double` | `0.1` | Seconds to wait for a needed transform before giving up on the frame. |
 | `initial_pose` | `string` | `""` | Starting pose, `"x y z roll pitch yaw"`. Also settable at runtime through `reset_odom_to_pose`. |
 | `ground_truth_frame_id` | `string` | `""` | When set, the pose is taken from this TF instead of being computed — for replaying a dataset with a known trajectory. |
-| `ground_truth_base_frame_id` | `string` | value of `frame_id` | The robot frame within the ground truth TF tree. |
+| `ground_truth_base_frame_id` | `string` | value of `frame_id` + `"_gt"` | The robot frame within the ground truth TF tree. Empty: the ground truth is not used. |
 | `guess_frame_id` | `string` | `""` | A frame carrying another odometry source, used as the initial guess for each registration. Documented with its companions under [Feeding in an external guess](#feeding-in-an-external-guess) -- **the highest-value parameter here for a wheeled robot**. |

 The sensor must be connected to `frame_id` in TF **before the first frame arrives**, or that frame is dropped with a warning. A static publisher is the usual answer.
@@ -177,7 +177,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
 	initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr); // "x y z roll pitch yaw"
 	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");
 	configPath_ = this->declare_parameter("config_path", configPath_);
 	publishNullWhenLost_ = this->declare_parameter("publish_null_when_lost", publishNullWhenLost_);

@@ -746,7 +746,7 @@ void OdometryROS::processData()
 			return;
 		}

-		if(!groundTruthFrameId_.empty())
+		if(!groundTruthFrameId_.empty() && !groundTruthBaseFrameId_.empty())
 		{
 			groundTruth = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, header.stamp, *tfBuffer_, waitForTransform_);

@@ -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;
 }

@@ -454,6 +454,63 @@ TEST_F(OdometryRosTest, starts_from_the_configured_initial_pose)
 	EXPECT_NEAR(3.0, odom->back().pose.pose.position.z, 1e-3);
 }

+/**
+ * With a ground truth frame, the odometry starts from the ground truth's first pose:
+ * ground_truth_frame_id -> ground_truth_base_frame_id, which defaults to frame_id + "_gt".
+ * Empty, ground_truth_base_frame_id turns the ground truth off.
+ */
+class OdometryRosGroundTruthTest : public OdometryRosTest
+{
+protected:
+	/// Latches world -> base_link_gt at (1, 2, 3), the only ground truth in TF.
+	void publishGroundTruth()
+	{
+		geometry_msgs::msg::TransformStamped gt;
+		gt.header.stamp = helper()->now();
+		gt.header.frame_id = "world";
+		gt.child_frame_id = "base_link_gt";
+		gt.transform.translation.x = 1.0;
+		gt.transform.translation.y = 2.0;
+		gt.transform.translation.z = 3.0;
+		gt.transform.rotation.w = 1.0;
+		gtTf_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(*helper());
+		gtTf_->sendTransform(gt);
+	}
+
+	/// The pose of the first frame, with the ground truth and @p params.
+	geometry_msgs::msg::Point firstPose(const std::vector<rclcpp::Parameter> & params)
+	{
+		publishSensorTf();
+		publishGroundTruth();
+		std::shared_ptr<Collector<nav_msgs::msg::Odometry>> odom =
+				collect<nav_msgs::msg::Odometry>("odom");
+		makeNode(params);
+		rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub = scanPublisher();
+		EXPECT_TRUE(waitForSubscriber(pub));
+		feedFrames(pub, odom, 1);
+		return odom->empty() ? geometry_msgs::msg::Point() : odom->back().pose.pose.position;
+	}
+
+	std::shared_ptr<tf2_ros::StaticTransformBroadcaster> gtTf_;
+};
+
+TEST_F(OdometryRosGroundTruthTest, starts_from_the_ground_truth_of_frame_id_gt_by_default)
+{
+	geometry_msgs::msg::Point p = firstPose({rclcpp::Parameter("ground_truth_frame_id", "world")});
+	EXPECT_NEAR(1.0, p.x, 1e-3);
+	EXPECT_NEAR(2.0, p.y, 1e-3);
+	EXPECT_NEAR(3.0, p.z, 1e-3);
+}
+
+TEST_F(OdometryRosGroundTruthTest, ignores_the_ground_truth_when_its_base_frame_is_empty)
+{
+	geometry_msgs::msg::Point p = firstPose({rclcpp::Parameter("ground_truth_frame_id", "world"),
+	                                         rclcpp::Parameter("ground_truth_base_frame_id", "")});
+	EXPECT_NEAR(0.0, p.x, 1e-3);
+	EXPECT_NEAR(0.0, p.y, 1e-3);
+	EXPECT_NEAR(0.0, p.z, 1e-3);
+}
+
 /// reset_odom puts the pose back to the identity and starts the map again.
 TEST_F(OdometryRosTest, reset_odom_returns_the_pose_to_the_origin)
 {