Skip to content

Commit

Permalink
WIP #13: Debugged transforms publisher
Browse files Browse the repository at this point in the history
  • Loading branch information
Jacob Gloudemans committed Feb 20, 2019
1 parent 6cfde5f commit 691f65a
Show file tree
Hide file tree
Showing 2 changed files with 62 additions and 43 deletions.
29 changes: 20 additions & 9 deletions robot_misc_tools/scripts/static_transforms.json
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand Down
76 changes: 42 additions & 34 deletions robot_misc_tools/scripts/transforms.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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__":
Expand All @@ -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)
Expand Down

0 comments on commit 691f65a

Please sign in to comment.