ROS2 various QOL updates (preparing new binary release) (#1225)

* Updating examples and README

* updated main readme

* odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning.

* update readme

* Added husky demo

* converted demo_robot_mapping.launch to ros2

* Converted stereo_outdoor demo from ros1 to ros2

* Converted multi-session demo to ros2

* Converted demo_find_object launch to ros2

* renamed files

* uniformized default qos, increased sync_queue_size default to 10

* Added vlp16 + imu example

* Added husky 3D lidar demos

* moved demos in subdir

* Added vlp16+zed example

* rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo.

* forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo

* pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces

* Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them.

* Added isaac sim demo

* Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty.

* Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo.

* rtabmap_demos: added README with examples

* Added TOC

* bump version to 0.21.9

rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp

@@ -36,6 +36,7 @@ namespace rtabmap_sync {
 void CommonDataSubscriber::rgbdCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -61,6 +62,7 @@ void CommonDataSubscriber::rgbdScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -85,6 +87,7 @@ void CommonDataSubscriber::rgbdScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -109,6 +112,7 @@ void CommonDataSubscriber::rgbdScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -132,6 +136,7 @@ void CommonDataSubscriber::rgbdInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -158,6 +163,7 @@ void CommonDataSubscriber::rgbdOdomCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -183,6 +189,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -207,6 +214,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -231,6 +239,7 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -258,6 +267,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -284,6 +294,7 @@ void CommonDataSubscriber::rgbdDataCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -309,6 +320,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -333,6 +345,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -357,6 +370,7 @@ void CommonDataSubscriber::rgbdDataScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -384,6 +398,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -410,6 +425,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
 		const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -435,6 +451,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -459,6 +476,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -483,6 +501,7 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
 
@@ -510,6 +529,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
 		const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
 		const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
 {
+	if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
 	cv_bridge::CvImageConstPtr rgb, depth;
 	rtabmap_conversions::toCvShare(image1Msg, rgb, depth);