From 02f228c2fd6e83437f04808b4209fabb72dbb30f Mon Sep 17 00:00:00 2001 From: light5551 Date: Tue, 30 Mar 2021 20:33:49 +0300 Subject: [PATCH 1/3] add fake_tag without hardcoded values --- .../src/apriltag_postprocessing_node.py | 36 +++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index c963c7c..5342af8 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -67,8 +67,9 @@ def __init__(self): self.sub_prePros = rospy.Subscriber("~detections", AprilTagDetectionArray, self.callback, queue_size=1) self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1) - self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1) - + self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1) + self.fake_tag_publisher = rospy.Publisher("~detections", AprilTagDetectionArray, queue_size=1) + # Need to call fake_tag, when FSM_state changed to intersection # topics for state machine self.pub_postPros_parking = rospy.Publisher("~apriltags_parking", BoolStamped, queue_size=1) self.pub_postPros_intersection = rospy.Publisher("~apriltags_intersection", BoolStamped, queue_size=1) @@ -80,6 +81,37 @@ def setupParam(self, param_name, default_value=rospy.client._Unspecified): rospy.set_param(param_name, value) return value + def fake_tag(): + tags_msg = AprilTagDetectionArray() + tags_msg.header.stamp = rospy.Time.now() + tags_msg.header.frame_id = "" + tag = AprilTagDetection( + transform=Transform( + translation=Vector3( + x=p[0], # Here will be hard coded values + y=p[1], + z=p[2] + ), + rotation=Quaternion( + x=q[0], + y=q[1], + z=q[2], + w=q[3] + ) + ), + tag_id=tag.tag_id, + tag_family=str(tag.tag_family), + hamming=tag.hamming, + decision_margin=tag.decision_margin, + homography=tag.homography.flatten().astype(np.float32).tolist(), + center=tag.center.tolist(), + corners=tag.corners.flatten().tolist(), + pose_error=tag.pose_err + ) + # add detection to array + tags_msg.detections.append(detection) + self.fake_tag_publish(tags_msg) + def callback(self, msg): tag_infos = [] From 24ea8ac902c5071fab269633cfa0f314697b4fc6 Mon Sep 17 00:00:00 2001 From: light5551 Date: Wed, 31 Mar 2021 22:19:08 +0300 Subject: [PATCH 2/3] move without apriltag --- .../packages/apriltag/src/apriltag_postprocessing_node.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index 56abdc4..21d1ba0 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -147,9 +147,7 @@ def fake_tag(self): tags_msg.detections.append(tag) self.fake_tag_publisher.publish(tags_msg) - def callback(self, msg): - - def callbackDetections(self, msg): + def callbackDetections(self, msg): tag_infos = [] new_tag_data = AprilTagsWithInfos() # Load tag detections message From b49d739563a8e6e8a971d866b51ee7d13d2887f9 Mon Sep 17 00:00:00 2001 From: light5551 Date: Wed, 31 Mar 2021 22:41:10 +0300 Subject: [PATCH 3/3] refactoring --- .../src/apriltag_postprocessing_node.py | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py index 21d1ba0..1c94253 100644 --- a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py +++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py @@ -117,33 +117,6 @@ def fake_tag(self): tag_id=58, tag_family="tag36h11" ) - ''' - tag = AprilTagDetection( - transform=Transform( - translation=Vector3( - x=0.17, # Here will be hard coded values - y=-0.13, - z=0.11 - ), - rotation=Quaternion( - x=0.67, - y=0.73, - z=-0.03, - w=0.05 - ) - ), - tag_id=58, - tag_family="tag36h11", - hamming=0, - decision_margin=63.566, - homography=[-41.83, 10.072, 441.941, -22.281, 43.970, 154.238, -0.1530, 0.0357], - center=[441.9419, 154.3480], - corners=[415.42, 185.73, 464.715, 199.214, 480.802, 108.3516, 423.951, 118.906], - pose_error=0.000000046 - ) - ''' - - # add detection to array tags_msg.detections.append(tag) self.fake_tag_publisher.publish(tags_msg)