From 8696ad14a57792286ed7f8caa1f20b0030504592 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Wed, 4 Sep 2019 19:14:55 +0300
Subject: [PATCH] =?UTF-8?q?aruco=5Fpose:=20don=E2=80=99t=20publish=20trans?=
 =?UTF-8?q?forms=20of=20markers=20in=20the=20map?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 aruco_pose/src/aruco_detect.cpp | 16 ++++++++++++++--
 clever/launch/aruco.launch      |  1 +
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp
index 92525e8a..eca65427 100644
--- a/aruco_pose/src/aruco_detect.cpp
+++ b/aruco_pose/src/aruco_detect.cpp
@@ -19,6 +19,7 @@
 #include <string>
 #include <map>
 #include <unordered_map>
+#include <unordered_set>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -62,12 +63,14 @@ private:
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
+	ros::Subscriber map_markers_sub_;
 	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
+	std::unordered_set<int> map_markers_ids_;
 	visualization_msgs::MarkerArray vis_array_;
 
 public:
@@ -101,6 +104,7 @@ public:
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);
 
+		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
 		debug_pub_ = it_priv.advertise("debug", 1);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
@@ -186,8 +190,8 @@ private:
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
 
-						// check if such static transform exists
-						if (!tf_buffer_.canTransform(transform.header.frame_id, transform.child_frame_id, transform.header.stamp)) {
+						// check if such static transform is in the map
+						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
 							transform.transform.rotation = marker.pose.orientation;
 							fillTranslation(transform.transform.translation, tvecs[i]);
 							br_.sendTransform(transform);
@@ -329,6 +333,14 @@ private:
 			return length_;
 		}
 	}
+
+	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
+	{
+		map_markers_ids_.clear();
+		for (auto const& marker : msg.markers) {
+			map_markers_ids_.insert(marker.id);
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)
diff --git a/clever/launch/aruco.launch b/clever/launch/aruco.launch
index c996a9fd..8d1a0e86 100644
--- a/clever/launch/aruco.launch
+++ b/clever/launch/aruco.launch
@@ -9,6 +9,7 @@
     <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
         <param name="known_tilt" value="map"/>