From 3c78963eb41b7dbad9704bec6f83213aaaa66c31 Mon Sep 17 00:00:00 2001 From: Max Hunt Date: Mon, 10 Feb 2020 14:09:44 +0000 Subject: [PATCH] Update --- main.py | 55 ++++++++++++++++++++++---------------------- object_tf_service.py | 33 ++++++++++++++------------ 2 files changed, 45 insertions(+), 43 deletions(-) diff --git a/main.py b/main.py index fad7315..1139329 100755 --- a/main.py +++ b/main.py @@ -30,7 +30,7 @@ table = { } bricks_end =[{ - 'id':'a1', + 'id':'ea1', 'rframe':'t1', 'x':0.46, 'y':-0.19, @@ -40,7 +40,7 @@ bricks_end =[{ 'yaw':radians(90) }, { - 'id':'a2', + 'id':'ea2', 'rframe':'a1', 'x':0, 'y':0, @@ -50,7 +50,7 @@ bricks_end =[{ 'yaw':0 }, { - 'id':'a3', + 'id':'ea3', 'rframe':'a1', 'x':0, 'y':0, @@ -60,7 +60,7 @@ bricks_end =[{ 'yaw':0 }, { - 'id':'b1', + 'id':'eb1', 'rframe':'a1', 'x':0.127, 'y':0, @@ -70,7 +70,7 @@ bricks_end =[{ 'yaw':0 }, { - 'id':'b2', + 'id':'eb2', 'rframe':'a1', 'x':0.127, 'y':0, @@ -80,7 +80,7 @@ bricks_end =[{ 'yaw':0 }, { - 'id':'c1', + 'id':'ec1', 'rframe':'a1', 'x':0.254, 'y':0, @@ -90,7 +90,7 @@ bricks_end =[{ 'yaw':0 }, { - 'id':'c2', + 'id':'ec2', 'rframe':'a1', 'x':0.254, 'y':0, @@ -100,7 +100,7 @@ bricks_end =[{ 'yaw':0 }, { - 'id':'d1', + 'id':'ed1', 'rframe':'a1', 'x':0.381, 'y':0, @@ -110,7 +110,7 @@ bricks_end =[{ 'yaw':0 }, { - 'id':'e1', + 'id':'ee1', 'rframe':'a1', 'x':0.508, 'y':0, @@ -121,7 +121,7 @@ bricks_end =[{ }] bricks_start =[{ - 'id':'ia1', + 'id':'a1', 'rframe':'t1', 'x':-0.419, 'y':0.134, @@ -131,7 +131,7 @@ bricks_start =[{ 'yaw':radians(90) }, { - 'id':'ia2', + 'id':'a2', 'rframe':'ia1', 'x':0, 'y':0, @@ -141,7 +141,7 @@ bricks_start =[{ 'yaw':0 }, { - 'id':'ia3', + 'id':'a3', 'rframe':'ia1', 'x':0, 'y':0, @@ -151,7 +151,7 @@ bricks_start =[{ 'yaw':0 }, { - 'id':'ib1', + 'id':'b1', 'rframe':'ia1', 'x':0, 'y':0.087, @@ -161,7 +161,7 @@ bricks_start =[{ 'yaw':0 }, { - 'id':'ib2', + 'id':'b2', 'rframe':'ia1', 'x':0, 'y':0.087, @@ -171,7 +171,7 @@ bricks_start =[{ 'yaw':0 }, { - 'id':'ic1', + 'id':'c1', 'rframe':'ia1', 'x':0, 'y':0.087, @@ -181,7 +181,7 @@ bricks_start =[{ 'yaw':0 }, { - 'id':'ic2', + 'id':'c2', 'rframe':'ia1', 'x':0, 'y':0.175, @@ -191,7 +191,7 @@ bricks_start =[{ 'yaw':0 }, { - 'id':'id1', + 'id':'d1', 'rframe':'ia1', 'x':0, 'y':0.175, @@ -201,7 +201,7 @@ bricks_start =[{ 'yaw':0 }, { - 'id':'ie1', + 'id':'e1', 'rframe':'ia1', 'x':0, 'y':0.175, @@ -363,16 +363,15 @@ def load_objects(): table_reference_frame="world" spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame) brick_ents = [] - for ar in [bricks_start, bricks_end]: - for brick in ar: - ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation - brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z'])) - brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z']) - bqo = quaternion_from_euler(brick['roll'], brick['pitch'], brick['yaw']) - brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3]) - brick_reference_frame=brick['rframe'] - brick_id = brick['id'] - brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame)) + for brick in bricks_start: + ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation + brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z'])) + brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z']) + bqo = quaternion_from_euler(brick['roll'], brick['pitch'], brick['yaw']) + brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3]) + brick_reference_frame=brick['rframe'] + brick_id = brick['id'] + brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame)) diff --git a/object_tf_service.py b/object_tf_service.py index 53f619b..aa2289e 100755 --- a/object_tf_service.py +++ b/object_tf_service.py @@ -6,20 +6,27 @@ from gazebo_msgs.msg import LinkStates class tf_service(): def __init__(self): self._nodes = ['baxter::base', - 't1::Table'] - # 'ia1::Brick'] - # 'ia2::Brick', - # 'ia3::Brick', - # 'ib1::Brick', - # 'ib2::Brick', - # 'ic1::Brick', - # 'ic2::Brick', - # 'id1::Brick', - # 'ie1::Brick'] + 't1::Table', + 'a1::Brick', + 'a2::Brick', + 'a3::Brick', + 'b1::Brick', + 'b2::Brick', + 'c1::Brick', + 'c2::Brick', + 'd1::Brick', + 'e1::Brick'] def _callback(self, data): # Get index for object_name in self._nodes: + '''HORRIBLE HACK BEGIN''' + if object_name == 'baxter::base': + spawn_name = 'base' + else: + spawn_name = object_name[:2] + + '''HORRIBLE HACK END''' # print(object_name) object_idx = data.name.index(object_name) # Get baxter_base pose and object pose w.r.t. gazebo world @@ -28,11 +35,7 @@ class tf_service(): br = tf.TransformBroadcaster() br.sendTransform((object_pose.position.x, object_pose.position.y, object_pose.position.z), (object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z, object_pose.orientation.w), - rospy.Time.now(), object_name,'gazebo_world') - - # print((object_pose.position.x, object_pose.position.y, object_pose.position.z), - # (object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z, object_pose.orientation.w), - # rospy.Time.now(), object_name,'gazebo_world') + rospy.Time.now(), spawn_name,'gazebo_world') def gazebo_link_subscriber(self):