diff --git a/stretch_funmap/nodes/aruco_head_scan_action.py b/stretch_funmap/nodes/aruco_head_scan_action.py index b1413ef..07b2d49 100755 --- a/stretch_funmap/nodes/aruco_head_scan_action.py +++ b/stretch_funmap/nodes/aruco_head_scan_action.py @@ -98,17 +98,18 @@ class ArucoHeadScan(hm.HelloNode): if self.aruco_name == 'docking_station': # Transform the docking station frame such that x-axis points out of the aruco plane and 0.5 m in the front of the dock # This facilitates passing the goal pose as this predock frame so that the robot can back up into the dock - t = Transform() - t.translation.x = 0.0 - t.translation.y = -0.25 - t.translation.z = 0.5 - t.rotation.x = 0.5 - t.rotation.y = 0.5 - t.rotation.z = 0.5 - t.rotation.w = -0.5 - tran = self.broadcast_tf(t, 'predock_pose', self.aruco_name) + saved_pose = Transform() + saved_pose.translation.x = 0.0 + saved_pose.translation.y = -0.45 + saved_pose.translation.z = 0.47 + saved_pose.rotation.x = -0.382 + saved_pose.rotation.y = -0.352 + saved_pose.rotation.z = -0.604 + saved_pose.rotation.w = 0.604 + tran = self.broadcast_tf(saved_pose, 'predock_pose', 'docking_station') self.tf2_broadcaster.sendTransform(tran) trans = self.tfBuffer.lookup_transform('map', 'predock_pose', rospy.Time()) + # Bring predock_frame at base_link level trans.transform.translation.z = 0 trans.header.stamp = rospy.Time.now() self.predock_tf = trans