From 691f65a6ad37323d41a3f406e164b037c3090590 Mon Sep 17 00:00:00 2001 From: Jacob Gloudemans Date: Tue, 19 Feb 2019 20:32:23 -0600 Subject: [PATCH] WIP #13: Debugged transforms publisher --- .../scripts/static_transforms.json | 29 ++++--- robot_misc_tools/scripts/transforms.py | 76 ++++++++++--------- 2 files changed, 62 insertions(+), 43 deletions(-) diff --git a/robot_misc_tools/scripts/static_transforms.json b/robot_misc_tools/scripts/static_transforms.json index a6f04fc..56ce929 100644 --- a/robot_misc_tools/scripts/static_transforms.json +++ b/robot_misc_tools/scripts/static_transforms.json @@ -12,7 +12,7 @@ "rot_w" : 1 }, { - "frame_id" : "robot_front_left", + "frame_id" : "aruco_camera", "child_id" : "robot_frame", "trans_x" : 0, "trans_y" : 0, @@ -23,8 +23,8 @@ "rot_w" : 1 }, { - "frame_id" : "robot_front_right", - "child_id" : "robot_frame", + "frame_id" : "robot_frame", + "child_id" : "robot_front_left", "trans_x" : 0, "trans_y" : 0, "trans_z" : 0, @@ -34,8 +34,8 @@ "rot_w" : 1 }, { - "frame_id" : "robot_back_left", - "child_id" : "robot_frame", + "frame_id" : "robot_frame", + "child_id" : "robot_front_right", "trans_x" : 0, "trans_y" : 0, "trans_z" : 0, @@ -45,8 +45,8 @@ "rot_w" : 1 }, { - "frame_id" : "robot_back_right", - "child_id" : "robot_frame", + "frame_id" : "robot_frame", + "child_id" : "robot_back_left", "trans_x" : 0, "trans_y" : 0, "trans_z" : 0, @@ -56,8 +56,19 @@ "rot_w" : 1 }, { - "frame_id" : "robot_kinect", - "child_id" : "robot_frame", + "frame_id" : "robot_frame", + "child_id" : "robot_back_right", + "trans_x" : 0, + "trans_y" : 0, + "trans_z" : 0, + "rot_x" : 0, + "rot_y" : 0, + "rot_z" : 0, + "rot_w" : 1 + }, + { + "frame_id" : "robot_frame", + "child_id" : "kinect_base", "trans_x" : 0, "trans_y" : 0, "trans_z" : 0, diff --git a/robot_misc_tools/scripts/transforms.py b/robot_misc_tools/scripts/transforms.py index 884f035..cbed84a 100644 --- a/robot_misc_tools/scripts/transforms.py +++ b/robot_misc_tools/scripts/transforms.py @@ -19,13 +19,13 @@ def aruco_pose_callback(pose_msg): tf_msg.header.stamp = rospy.Time.now() tf_msg.header.frame_id = "aruco_board_origin" tf_msg.child_frame_id = "aruco_camera" - tf_msg.transform.translation.x = pose_msg.pose.position.x - tf_msg.transform.translation.y = pose_msg.pose.position.y - tf_msg.transform.translation.z = pose_msg.pose.position.z - tf_msg.transform.rotation.x = pose_msg.pose.orientation.x - tf_msg.transform.rotation.y = pose_msg.pose.orientation.y - tf_msg.transform.rotation.z = pose_msg.pose.orientation.z - tf_msg.transform.rotation.w = pose_msg.pose.orientation.w + tf_msg.transform.translation.x = pose_msg.position.x + tf_msg.transform.translation.y = pose_msg.position.y + tf_msg.transform.translation.z = pose_msg.position.z + tf_msg.transform.rotation.x = pose_msg.orientation.x + tf_msg.transform.rotation.y = pose_msg.orientation.y + tf_msg.transform.rotation.z = pose_msg.orientation.z + tf_msg.transform.rotation.w = pose_msg.orientation.w # Publish transform b.sendTransform(tf_msg) @@ -38,42 +38,50 @@ def camera_angle_callback(pose_msg): # Create transform message tf_msg = tf2_ros.TransformStamped() tf_msg.header.stamp = rospy.Time.now() - tf_msg.header.frame_id = "robot_frame" - tf_msg.child_frame_id = "aruco_camera" + tf_msg.header.frame_id = "aruco_camera" + tf_msg.child_frame_id = "robot_frame" tf_msg.transform.translation.x = 0 #define these with CAD files tf_msg.transform.translation.y = 0 #define these with CAD files tf_msg.transform.translation.z = 0 #define these with CAD files - tf_msg.transform.rotation.x = pose_msg.pose.orientation.x - tf_msg.transform.rotation.y = pose_msg.pose.orientation.y - tf_msg.transform.rotation.z = pose_msg.pose.orientation.z - tf_msg.transform.rotation.w = pose_msg.pose.orientation.w + tf_msg.transform.rotation.x = pose_msg.orientation.x + tf_msg.transform.rotation.y = pose_msg.orientation.y + tf_msg.transform.rotation.z = pose_msg.orientation.z + tf_msg.transform.rotation.w = pose_msg.orientation.w # Publish transform b.sendTransform(tf_msg) # getStaticTransform returns a TransformStamped filled with values defined in parameter server # Parameter server key should be /transform/{TRANSFORM_NAME} -def getStaticTransform(paramServerDict): - tf = tf2_ros.TransformStamped() - tf.stamp = rospy.Time.now() +def getStaticTransform(transformList): + + staticTransformList = [] + + for transform in transformList: + tf = tf2_ros.TransformStamped() + tf.header.stamp = rospy.Time.now() + + # Read in frame ids from parameter server + tf.header.frame_id = transform["frame_id"] + tf.child_frame_id = transform["child_id"] - # Read in frame ids from parameter server - tf.header.frame_id = paramServerDict["frame_id"] - tf.child_frame_id = paramServerDict["child_frame_id"] + print tf.header.frame_id + print tf.child_frame_id - # Read in translation from parameter server - tf.transform.translation.x = paramServerDict["trans_x"] - tf.transform.translation.y = paramServerDict["trans_y"] - tf.transform.translation.z = paramServerDict["trans_z"] + # Read in translation fro_m parameter server + tf.transform.translation.x = transform["trans_x"] + tf.transform.translation.y = transform["trans_y"] + tf.transform.translation.z = transform["trans_z"] - # Read in rotation from parameter server - tf.transform.rotation.x = paramServerDict["rot_x"] - tf.transform.rotation.y = paramServerDict["rot_y"] - tf.transform.rotation.z = paramServerDict["rot_z"] - tf.transform.rotation.w = paramServerDict["rot_w"] + # Read in rotation from parameter server + tf.transform.rotation.x = transform["rot_x"] + tf.transform.rotation.y = transform["rot_y"] + tf.transform.rotation.z = transform["rot_z"] + tf.transform.rotation.w = transform["rot_w"] - return tf + staticTransformList.append(tf) + return staticTransformList if __name__ == "__main__": @@ -86,18 +94,18 @@ def getStaticTransform(paramServerDict): # Marker board transform (relative to world - FIXED) - broadcaster = tf2_ros.StaticTransformBroadcaster() # Read in transform file from parameter server - transformFile = rospy.get_param("/transform_file") - with open("transformFile") as f: + transformFile = rospy.get_param("/transform_file", default="static_transforms.json") + with open(transformFile) as f: transformDict = json.load(f) # Extract list of transform objects transformList = transformDict["transforms"] - for transform in transformList: - broadcaster.sendTransform(getStaticTransform(transform)) + + broadcaster = tf2_ros.StaticTransformBroadcaster() + broadcaster.sendTransform(getStaticTransform(transformList)) # Aruco marker camera transform (relative to marker board)