max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-10 14:09:44 +00:00
parent 1637350d18
commit 3c78963eb4
2 changed files with 45 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
main.py
View File

@ -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,8 +363,7 @@ 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:
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'])

33
object_tf_service.py
View File

@ -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):