rtabmap_odom tests and doc (#1456)

* rtabmap_odom tests and doc * opengv note * added ci checks or humble-latest flaky dep cmake errors * Added real data tests for rgbd_odom and stereo_odom * added real data for icp_odometry's deskewing test * fixing json cmake error on lyrical/rolling * test 2d icp odom deskewing branch * first review of existing OdometryROS tests * testing with imu used as guess * tested imu arrivals sync * Fixed odom reset on right pose when guess frame id is used * fixing header errors in ci >=lyrical * Added support for input rgbd_image topic with features for odom, added multicam rgbd_odometry test * Added stereo odom support for features-only frames. Added multicam stereo tests. * forcing latest rtabmap version * updated OdometryROS API * ci: dont build non-latest docker in pull requests * splitting docker jobs * doc edit * Making publish_null_when_lost:=false continous when guess is provided (using guess covariance when we cannot register yet) * updated stereo doc * ficing rolling ci (rviz Ogre header) * Added test coverage of alll rgbd_image callbacks * fixing rolling ci * making docker ci build/run the tests on pull requests * fixing ros2 ci testing * improved sync callback coverage * improving stereo_odometry test coverage * improved icp_odometry test coverage * lyrical voxel_grid ptr error * make multicam tests working as well without opengv * removing deps of missing packages on rolling * PCL empty cloud conversion compiler errors fix * fixing icp_odometry test failure on ci witohut libpointmatcher * fixing nav2 costmap plugin build on lyrical * joining thread when exiting * updating icp test to work the same on pcl 1.15 (lyrical) * Fix parallel tests seg fault --------- Co-authored-by: mathieu86 <[email protected]>
2026-10-04 08:47:45 +08:00 · 2026-09-21 17:02:45 -07:00
parent 73c98f87a8
commit 11edc01d6a
91 changed files with 9736 additions and 350 deletions
@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2026, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved. (BSD-3-Clause, see the repository root.)
+*/
+
+#ifndef RTABMAP_ODOM_BAG_PLAYBACK_HPP_
+#define RTABMAP_ODOM_BAG_PLAYBACK_HPP_
+
+#include <rclcpp/serialization.hpp>
+#include <rosbag2_cpp/reader.hpp>
+
+#include <string>
+#include <vector>
+
+#include "test_data.hpp"
+
+/**
+ * @file
+ * @brief Reads recorded messages out of a bag, for tests that need real sensor input.
+ *
+ * The messages are read and replayed by the test itself rather than by `ros2 bag play`:
+ * no second process, no wall-clock pacing, and the test controls exactly when each
+ * message reaches the node. What the recording provides is the part that cannot be
+ * written by hand -- a real driver's cloud layout and a dense TF history around it.
+ */
+
+namespace rtabmap_odom_test {
+
+/**
+ * @brief The Ouster recording in test/data/lidar; see that directory's README.
+ *
+ * Two sweeps half a mast turn apart, from a platform that never moves: the only thing
+ * between them is the mast's rotation, which TF describes in full.
+ */
+inline std::string ousterHalfTurnBag()
+{
+	return testDataRoot() + "/lidar/ouster_half_turn";
+}
+
+/**
+ * @brief Every message recorded on @p topic, deserialized.
+ *
+ * Returns an empty vector if the bag or the topic is missing, which the caller is
+ * expected to assert on -- a silently empty fixture would make a test pass for the wrong
+ * reason.
+ */
+template <typename MsgT>
+std::vector<MsgT> readBagMessages(const std::string & bagPath, const std::string & topic)
+{
+	std::vector<MsgT> messages;
+	rosbag2_cpp::Reader reader;
+	try
+	{
+		reader.open(bagPath);
+	}
+	catch(const std::exception & e)
+	{
+		return messages;
+	}
+
+	rclcpp::Serialization<MsgT> serialization;
+	while(reader.has_next())
+	{
+		const std::shared_ptr<rosbag2_storage::SerializedBagMessage> message = reader.read_next();
+		if(message->topic_name != topic)
+		{
+			continue;
+		}
+		rclcpp::SerializedMessage serialized(*message->serialized_data);
+		MsgT deserialized;
+		serialization.deserialize_message(&serialized, &deserialized);
+		messages.push_back(deserialized);
+	}
+	return messages;
+}
+
+}  // namespace rtabmap_odom_test
+
+#endif /* RTABMAP_ODOM_BAG_PLAYBACK_HPP_ */