Fixing topic sync lagging and delay issues, more examples (depthai, zed) (#1206)

* Fixed odometry latency

* Fixed node ID=0 issues as msgs may not have seq. (#1202)

(cherry picked from commit 097cab0667)

* Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters)

* Changed a log from info->debug

* Changed a log from info->debug

* Updated default topic and sync queue_size inside nodes. Exposing topic and sync queue size params in warning when cannot synchronize. Added zed and depthai examples. Odom: Fixed imu callback group, added multi-threaded executors for all odometry nodes.

* fixed merge

* Include everything needed in example launch files for simple launch. Small fixes.

---------

Co-authored-by: Borong Yuan <[email protected]>

rtabmap_odom/include/rtabmap_odom/OdometryROS.h

@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_msgs/srv/reset_pose.hpp>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/UThread.h>
 
 #include <boost/thread.hpp>
 
@@ -59,7 +60,7 @@ class Odometry;
 
 namespace rtabmap_odom {
 
-class OdometryROS : public rclcpp::Node
+class OdometryROS : public rclcpp::Node, public UThread
 {
 
 public:
@@ -98,12 +99,17 @@ protected:
 
 private:
 
+	virtual void mainLoop();
+	virtual void mainLoopKill();
 	virtual void updateParameters(rtabmap::ParametersMap &) {}
 	virtual void onOdomInit() {}
 
 	void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
 	void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
 
+protected:
+	rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
+
 private:
 	rtabmap::Odometry * odometry_;
 
@@ -147,6 +153,14 @@ private:
 	std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
 	std::shared_ptr<tf2_ros::TransformListener> tfListener_;
 	rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
+	rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_;
+
+	// Safe-threading
+	UMutex imuMutex_;
+	UMutex dataMutex_;	
+	USemaphore dataReady_;
+	rtabmap::SensorData dataToProcess_;
+	std_msgs::msg::Header dataHeaderToProcess_;
 
 	bool paused_;
 	int resetCountdown_;
@@ -166,7 +180,6 @@ private:
 	bool waitIMUToinit_;
 	bool imuProcessed_;
 	std::map<double, rtabmap::IMU> imus_;
-	std::pair<rtabmap::SensorData, std_msgs::msg::Header > bufferedData_;
 	std::string configPath_;
 	rtabmap::Transform initialPose_;