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_odom/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_odom/test/test_odometry_ros.cpp

@@ -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)
 {