Added apriltag_msgs support

rtabmap_launch/launch/rtabmap.launch.py

@@ -471,7 +471,7 @@ def generate_launch_description():
         DeclareLaunchArgument('gps_topic',  default_value='/gps/fix', description='GPS async subscription. This is used for SLAM graph optimization and loop closure candidates selection.'),
 
         # Tag/Landmark
-        DeclareLaunchArgument('tag_topic',            default_value='/tag_detections', description='AprilTag topic async subscription. This is used for SLAM graph optimization and loop closure detection. Landmark poses are also published accordingly to current optimized map.'),
+        DeclareLaunchArgument('tag_topic',            default_value='/detections', description='AprilTag topic async subscription. This is used for SLAM graph optimization and loop closure detection. Landmark poses are also published accordingly to current optimized map. Required: Remove optional frame name parameters from apriltag\'s cfg file so that TF frame can be deducted from topic\'s family and id.'),
         DeclareLaunchArgument('tag_linear_variance',  default_value='0.0001',          description=''),
         DeclareLaunchArgument('tag_angular_variance', default_value='9999.0',            description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'),
         DeclareLaunchArgument('fiducial_topic',       default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'),

rtabmap_slam/CMakeLists.txt

@@ -34,6 +34,7 @@ ENDIF()
 
 #optional
 find_package(octomap_msgs)
+find_package(apriltag_msgs)
 
 IF(WIN32)
 add_compile_options(-bigobj)
@@ -74,8 +75,22 @@ SET(rtabmap_slam_plugins_lib_src
 IF(octomap_msgs_FOUND)
 MESSAGE(STATUS "WITH octomap_msgs")
 ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS")
+SET(Libraries
+   ${Libraries}
+   octomap_msgs
+)
 ENDIF(octomap_msgs_FOUND)
 
+# If apriltag_msgs is found, add definition
+IF(apriltag_msgs_FOUND)
+MESSAGE(STATUS "WITH apriltag_msgs")
+ADD_DEFINITIONS("-DWITH_APRILTAG_MSGS")
+SET(Libraries
+   ${Libraries}
+   apriltag_msgs
+)
+ENDIF(apriltag_msgs_FOUND)
+
 ############################
 ## Declare a cpp library
 ############################

rtabmap_slam/src/CoreWrapper.cpp

@@ -809,7 +809,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	globalPoseAsyncSub_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 5, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1));
 	gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1));
 #ifdef WITH_APRILTAG_MSGS
-	tagDetectionsSub_ = this->create_subscription<apriltag_ros::msg::AprilTagDetectionArray>("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1));
+	tagDetectionsSub_ = this->create_subscription<apriltag_msgs::msg::AprilTagDetectionArray>("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1));
 #endif
 #ifdef WITH_FIDUCIAL_MSGS
 	fiducialTransfromsSub_ = this->create_subscription<fiducial_msgs::msg::FiducialTransformArray>("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1));
@@ -2293,44 +2293,29 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD
 	{
 		for(unsigned int i=0; i<tagDetections->detections.size(); ++i)
 		{
-			if(tagDetections->detections[i].id.size() >= 1)
+			std::string tagFrameId = tagDetections->detections[i].family+":"+uNumber2Str(tagDetections->detections[i].id);
+			Transform camToTag = rtabmap_conversions::getTransform(
+				tagDetections->header.frame_id, // e.g., camera_optical_frame
+				tagFrameId,                     // e.g., tag36h11:42
+				tagDetections->header.stamp,
+				*tfBuffer_,
+				waitForTransform_);
+			if(camToTag.isNull())
 			{
-				geometry_msgs::msg::PoseWithCovarianceStamped p = tagDetections->detections[i].pose;
-				p.header = tagDetections->header;
-				if(!tagDetections->detections[i].pose.header.frame_id.empty())
-				{
-					p.header.frame_id = tagDetections->detections[i].pose.header.frame_id;
-
-					static bool warned = false;
-					if(!warned &&
-						!tagDetections->header.frame_id.empty() &&
-						tagDetections->detections[i].pose.header.frame_id.compare(tagDetections->header.frame_id)!=0)
-					{
-						RCLCPP_WARN(get_logger(), "frame_id set for individual tag detections (%s) doesn't match the frame_id of the message (%s), "
-								"the resulting pose of the tag may be wrong. This message is only printed once.",
-								tagDetections->detections[i].pose.header.frame_id.c_str(), tagDetections->header.frame_id.c_str());
-						warned = true;
-					}
-				}
-				if(!tagDetections->detections[i].pose.header.stamp.isZero())
-				{
-					p.header.stamp = tagDetections->detections[i].pose.header.stamp;
-
-					static bool warned = false;
-					if(!warned &&
-						!tagDetections->header.stamp.isZero() &&
-						tagDetections->detections[i].pose.header.stamp != tagDetections->header.stamp)
-					{
-						RCLCPP_WARN(get_logger(), "stamp set for individual tag detections (%f) doesn't match the stamp of the message (%f), "
-								"the resulting pose of the tag may be wrongly interpolated. This message is only printed once.",
-								tagDetections->detections[i].pose.header.stamp.toSec(), tagDetections->header.stamp.toSec());
-						warned = true;
-					}
-				}
-				uInsert(tags_,
-						std::make_pair(tagDetections->detections[i].id[0],
-								std::make_pair(p, tagDetections->detections[i].size.size()==1?(float)tagDetections->detections[i].size[0]:0.0f)));
+				RCLCPP_WARN(get_logger(), "Could not get TF between %s and %s frames for tag detection %d.",
+					frameId_.c_str(),
+					tagFrameId.c_str(),
+					tagDetections->detections[i].id);
+					continue;
 			}
+
+			geometry_msgs::msg::PoseWithCovarianceStamped p;
+			rtabmap_conversions::transformToPoseMsg(camToTag, p.pose.pose);
+			p.header = tagDetections->header;
+			
+			uInsert(tags_,
+					std::make_pair(tagDetections->detections[i].id,
+							std::make_pair(p, 0.0f)));
 		}
 	}
 }