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_util/doc/lidar_deskewing.md

@@ -65,7 +65,8 @@ Output names are derived from the **resolved** input names, so remapping the inp
 | `fixed_frame_id` | `string` | `""` | **Required.** Frame the motion is measured against, usually `odom`. |
 | `wait_for_transform` | `double` | `0.01` | Seconds to wait for the transforms spanning the sweep. Raise it if odometry lags the lidar. |
 | `slerp` | `bool` | `false` | Interpolate between the poses at the start and end of the sweep instead of looking up TF per point. Much cheaper, and accurate enough at constant velocity. |
-| `queue_size` | `int` | `1` | Queue depth of the input subscriptions. |
+| `queue_size` | `int` | `5` | Queue depth of the input subscriptions. |
+| `output_queue_size` | `int` | `1` | History depth of the deskewed cloud publishers. |
 | `qos` | `int` | `0` | Reliability of the input subscriptions: `0` system default, `1` reliable, `2` best effort. |
 
 ## Requirements

rtabmap_util/src/nodelets/lidar_deskewing.cpp

@@ -19,6 +19,8 @@ LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
 	int queueSize = 5;
 	int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
 	queueSize = this->declare_parameter("queue_size", queueSize);
+	int outputQueueSize = 1;
+	outputQueueSize = this->declare_parameter("output_queue_size", outputQueueSize);
 	qos = this->declare_parameter("qos", qos);
 	fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
 	waitForTransformDuration_ = this->declare_parameter("wait_for_transform", waitForTransformDuration_);
@@ -36,8 +38,8 @@ LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
 	subScan_ = create_subscription<sensor_msgs::msg::LaserScan>("input_scan", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackScan, this, std::placeholders::_1));
 	subCloud_ = create_subscription<sensor_msgs::msg::PointCloud2>("input_cloud", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&LidarDeskewing::callbackCloud, this, std::placeholders::_1));
 
-	pubScan_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subScan_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
-	pubCloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subCloud_->get_topic_name()) + "/deskewed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
+	pubScan_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subScan_->get_topic_name()) + "/deskewed", rclcpp::QoS(outputQueueSize).reliability((rmw_qos_reliability_policy_t)qos));
+	pubCloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(std::string(subCloud_->get_topic_name()) + "/deskewed", rclcpp::QoS(outputQueueSize).reliability((rmw_qos_reliability_policy_t)qos));
 }
 
 LidarDeskewing::~LidarDeskewing()

rtabmap_util/test/node_test_utils.hpp

@@ -31,6 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <gtest/gtest.h>
 
 #include <rclcpp/rclcpp.hpp>
+
+#include <execinfo.h>
+#include <unistd.h>
+
+#include <csignal>
 #include <geometry_msgs/msg/transform_stamped.hpp>
 #include <tf2/LinearMath/Quaternion.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
@@ -71,11 +76,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;
 }