From ec9b77d0558725ddaa9d55a6f62d5d9aac2052b0 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 2 Dec 2021 11:40:13 -0500
Subject: [PATCH] Added fiducial_msgs support (aruco_detect), just connect to
 /fiducial_transforms input of rtabmap node.

---
 CMakeLists.txt                    | 14 ++++++++++++++
 include/rtabmap_ros/CoreWrapper.h |  9 +++++++++
 launch/rtabmap.launch             |  2 ++
 src/CoreWrapper.cpp               | 24 ++++++++++++++++++++++++
 4 files changed, 49 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 35f17948..ae8be8fc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,6 +28,7 @@ find_package(octomap_msgs)
 find_package(apriltag_ros)
 find_package(rviz)
 find_package(find_object_2d)
+find_package(fiducial_msgs)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -293,6 +294,19 @@ SET(Libraries
 ADD_DEFINITIONS("-DWITH_APRILTAG_ROS")
 ENDIF(apriltag_ros_FOUND)
 
+# If fiducial_msgs is found, add definition
+IF(fiducial_msgs_FOUND)
+MESSAGE(STATUS "WITH fiducial_msgs")
+include_directories(
+  ${fiducial_msgs_INCLUDE_DIRS}
+)
+SET(Libraries
+  ${fiducial_msgs_LIBRARIES}
+  ${Libraries}
+)
+ADD_DEFINITIONS("-DWITH_FIDUCIAL_MSGS")
+ENDIF(fiducial_msgs_FOUND)
+
 ############################
 ## Declare a cpp library
 ############################
diff --git a/include/rtabmap_ros/CoreWrapper.h b/include/rtabmap_ros/CoreWrapper.h
index 13bd0e75..5a072888 100644
--- a/include/rtabmap_ros/CoreWrapper.h
+++ b/include/rtabmap_ros/CoreWrapper.h
@@ -78,6 +78,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <apriltag_ros/AprilTagDetectionArray.h>
 #endif
 
+//#define WITH_FIDUCIAL_MSGS
+#ifdef WITH_FIDUCIAL_MSGS
+#include <fiducial_msgs/FiducialTransformArray.h>
+#endif
+
 #include <actionlib/client/simple_action_client.h>
 #include <move_base_msgs/MoveBaseAction.h>
 #include <move_base_msgs/MoveBaseActionGoal.h>
@@ -164,6 +169,9 @@ private:
 	void gpsFixAsyncCallback(const sensor_msgs::NavSatFixConstPtr & gpsFixMsg);
 #ifdef WITH_APRILTAG_ROS
 	void tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetectionArray & tagDetections);
+#endif
+#ifdef WITH_FIDUCIAL_MSGS
+	void fiducialDetectionsAsyncCallback(const fiducial_msgs::FiducialTransformArray & fiducialDetections);
 #endif
 	void imuAsyncCallback(const sensor_msgs::ImuConstPtr & tagDetections);
 	void republishNodeDataCallback(const std_msgs::Int32MultiArray::ConstPtr& msg);
@@ -368,6 +376,7 @@ private:
 	ros::Subscriber gpsFixAsyncSub_;
 	rtabmap::GPS gps_;
 	ros::Subscriber tagDetectionsSub_;
+	ros::Subscriber fiducialTransfromsSub_;
 	std::map<int, std::pair<geometry_msgs::PoseWithCovarianceStamped, float> > tags_; // id, <pose, size>
 	ros::Subscriber imuSub_;
 	std::map<double, rtabmap::Transform> imus_;
diff --git a/launch/rtabmap.launch b/launch/rtabmap.launch
index 69db25ba..f8bb245e 100644
--- a/launch/rtabmap.launch
+++ b/launch/rtabmap.launch
@@ -142,6 +142,7 @@
   <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
   <arg name="tag_linear_variance"      default="0.0001" />
   <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
+  <arg name="fiducial_topic"           default="/fiducial_transforms" />  <!-- aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covriance -->
   
   <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
   <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
@@ -382,6 +383,7 @@
       <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
       <remap from="gps/fix"                to="$(arg gps_topic)"/>
       <remap from="tag_detections"         to="$(arg tag_topic)"/>
+      <remap from="fiducial_transforms"    to="$(arg fiducial_topic)"/>
       <remap from="odom"                   to="$(arg odom_topic)"/>
       <remap from="imu"                    to="$(arg imu_topic)"/>
       
diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp
index 2a81dc6f..61329ee5 100644
--- a/src/CoreWrapper.cpp
+++ b/src/CoreWrapper.cpp
@@ -824,6 +824,9 @@ void CoreWrapper::onInit()
 	gpsFixAsyncSub_ = nh.subscribe("gps/fix", 1, &CoreWrapper::gpsFixAsyncCallback, this);
 #ifdef WITH_APRILTAG_ROS
 	tagDetectionsSub_ = nh.subscribe("tag_detections", 1, &CoreWrapper::tagDetectionsAsyncCallback, this);
+#endif
+#ifdef WITH_FIDUCIAL_MSGS
+	fiducialTransfromsSub_ = nh.subscribe("fiducial_transforms", 1, &CoreWrapper::fiducialDetectionsAsyncCallback, this);
 #endif
 	imuSub_ = nh.subscribe("imu", 100, &CoreWrapper::imuAsyncCallback, this);
 	republishNodeDataSub_ = nh.subscribe("republish_node_data", 100, &CoreWrapper::republishNodeDataCallback, this);
@@ -2468,6 +2471,27 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
 }
 #endif
 
+#ifdef WITH_FIDUCIAL_MSGS
+void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::FiducialTransformArray & fiducialDetections)
+{
+	if(!paused_)
+	{
+		for(unsigned int i=0; i<fiducialDetections.transforms.size(); ++i)
+		{
+			geometry_msgs::PoseWithCovarianceStamped p;
+			p.pose.pose.orientation = fiducialDetections.transforms[i].transform.rotation;
+			p.pose.pose.position.x = fiducialDetections.transforms[i].transform.translation.x;
+			p.pose.pose.position.y = fiducialDetections.transforms[i].transform.translation.y;
+			p.pose.pose.position.z = fiducialDetections.transforms[i].transform.translation.z;
+			p.header = fiducialDetections.header;
+			uInsert(tags_,
+					std::make_pair(fiducialDetections.transforms[i].fiducial_id,
+							std::make_pair(p, 0.0f)));
+		}
+	}
+}
+#endif
+
 void CoreWrapper::imuAsyncCallback(const sensor_msgs::ImuConstPtr & msg)
 {
 	if(!paused_)