From 421714ea306be1ea54cb88a075f7aeb34e370b79 Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Wed, 22 Feb 2023 18:24:53 -0500 Subject: [PATCH] Publish tf frame of dock to tf tree --- stretch_core/action/ArucoHeadScan.action | 3 +++ stretch_core/nodes/aruco_head_scan_client.py | 1 + stretch_core/nodes/detect_aruco_markers.py | 25 +++++++++++++++++++- 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/stretch_core/action/ArucoHeadScan.action b/stretch_core/action/ArucoHeadScan.action index 85e5ad6..8ae0ef4 100644 --- a/stretch_core/action/ArucoHeadScan.action +++ b/stretch_core/action/ArucoHeadScan.action @@ -6,6 +6,9 @@ uint32 aruco_id # Specify the camera tilt_angle at which to scan float32 tilt_angle +# Publish tf relative to the map frame +bool publish_to_map + # If robot should rotate to cover the blind spot generated by the mast bool fill_in_blindspot_with_second_scan diff --git a/stretch_core/nodes/aruco_head_scan_client.py b/stretch_core/nodes/aruco_head_scan_client.py index 9e29bd0..77fdb55 100755 --- a/stretch_core/nodes/aruco_head_scan_client.py +++ b/stretch_core/nodes/aruco_head_scan_client.py @@ -14,6 +14,7 @@ def main(): goal = ArucoHeadScanGoal() goal.aruco_id = 245 goal.tilt_angle = -0.68 + goal.publish_to_map = True goal.fill_in_blindspot_with_second_scan = False goal.fast_scan = False diff --git a/stretch_core/nodes/detect_aruco_markers.py b/stretch_core/nodes/detect_aruco_markers.py index d23dc0b..d1670cc 100755 --- a/stretch_core/nodes/detect_aruco_markers.py +++ b/stretch_core/nodes/detect_aruco_markers.py @@ -528,6 +528,8 @@ class ArucoHeadScan(hm.HelloNode): self.aruco_found = False self.markers = MarkerArray().markers self.joint_state = JointState() + self.listener = tf.TransformListener() + self.tf_broadcaster = tf.TransformBroadcaster() def execute_cb(self, goal): self.goal = goal @@ -535,6 +537,7 @@ class ArucoHeadScan(hm.HelloNode): self.tilt_angle = self.goal.tilt_angle self.fill_in_blindspot_with_second_scan = self.goal.fill_in_blindspot_with_second_scan self.fast_scan = self.goal.fast_scan + self.publish_to_map = self.goal.publish_to_map pan_angle = 0.0 scan_point = JointTrajectoryPoint() @@ -555,13 +558,26 @@ class ArucoHeadScan(hm.HelloNode): # TODO: check distance of aruco to base_link < 2m for pan_angle in np.arange(-3.69, 1.66, 0.35): - time.sleep(1.0) + time.sleep(2.0) markers = self.markers rospy.loginfo("Markers found: {}".format(markers)) + if markers != []: for marker in markers: if marker.id == self.aruco_id: self.aruco_found = True + self.aruco_name = marker.text + if self.publish_to_map: + (trans,rot) = self.listener.lookupTransform('map', self.aruco_name, rospy.Time(0)) + rospy.loginfo("trans: {}".format(trans)) + rospy.loginfo("rot: {}".format(rot)) + self.broadcast_tf(trans, rot, self.aruco_name, 'map') + if self.aruco_name == 'docking_station': + rospy.loginfo("Publishing predock pose") + self.broadcast_tf([0.0, -0.249, 1.0], [0.0, 0.0, 0.0, 1.0], 'predock_pose', 'docking_station') + rospy.loginfo("Publishing dock pose") + self.broadcast_tf([0.0, -0.249, 0.25], [0.0, 0.0, 0.0, 1.0], 'dock_pose', 'docking_station') + if not self.aruco_found: self.feedback.pan_angle = pan_angle self.server.publish_feedback(self.feedback) @@ -588,6 +604,13 @@ class ArucoHeadScan(hm.HelloNode): rospy.loginfo("Could not find aruco marker {}".format(str)) self.server.set_aborted(self.result) + def broadcast_tf(self, pos, rot, name, ref): + self.tf_broadcaster.sendTransform(pos, + rot, + rospy.Time.now(), + name, + ref) + def aruco_callback(self, msg): self.markers = msg.markers