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 =[{ bricks_end =[{
'id':'a1', 'id':'ea1',
'rframe':'t1', 'rframe':'t1',
'x':0.46, 'x':0.46,
'y':-0.19, 'y':-0.19,
@ -40,7 +40,7 @@ bricks_end =[{
'yaw':radians(90) 'yaw':radians(90)
}, },
{ {
'id':'a2', 'id':'ea2',
'rframe':'a1', 'rframe':'a1',
'x':0, 'x':0,
'y':0, 'y':0,
@ -50,7 +50,7 @@ bricks_end =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'a3', 'id':'ea3',
'rframe':'a1', 'rframe':'a1',
'x':0, 'x':0,
'y':0, 'y':0,
@ -60,7 +60,7 @@ bricks_end =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'b1', 'id':'eb1',
'rframe':'a1', 'rframe':'a1',
'x':0.127, 'x':0.127,
'y':0, 'y':0,
@ -70,7 +70,7 @@ bricks_end =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'b2', 'id':'eb2',
'rframe':'a1', 'rframe':'a1',
'x':0.127, 'x':0.127,
'y':0, 'y':0,
@ -80,7 +80,7 @@ bricks_end =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'c1', 'id':'ec1',
'rframe':'a1', 'rframe':'a1',
'x':0.254, 'x':0.254,
'y':0, 'y':0,
@ -90,7 +90,7 @@ bricks_end =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'c2', 'id':'ec2',
'rframe':'a1', 'rframe':'a1',
'x':0.254, 'x':0.254,
'y':0, 'y':0,
@ -100,7 +100,7 @@ bricks_end =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'d1', 'id':'ed1',
'rframe':'a1', 'rframe':'a1',
'x':0.381, 'x':0.381,
'y':0, 'y':0,
@ -110,7 +110,7 @@ bricks_end =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'e1', 'id':'ee1',
'rframe':'a1', 'rframe':'a1',
'x':0.508, 'x':0.508,
'y':0, 'y':0,
@ -121,7 +121,7 @@ bricks_end =[{
}] }]
bricks_start =[{ bricks_start =[{
'id':'ia1', 'id':'a1',
'rframe':'t1', 'rframe':'t1',
'x':-0.419, 'x':-0.419,
'y':0.134, 'y':0.134,
@ -131,7 +131,7 @@ bricks_start =[{
'yaw':radians(90) 'yaw':radians(90)
}, },
{ {
'id':'ia2', 'id':'a2',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0, 'y':0,
@ -141,7 +141,7 @@ bricks_start =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'ia3', 'id':'a3',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0, 'y':0,
@ -151,7 +151,7 @@ bricks_start =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'ib1', 'id':'b1',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0.087, 'y':0.087,
@ -161,7 +161,7 @@ bricks_start =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'ib2', 'id':'b2',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0.087, 'y':0.087,
@ -171,7 +171,7 @@ bricks_start =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'ic1', 'id':'c1',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0.087, 'y':0.087,
@ -181,7 +181,7 @@ bricks_start =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'ic2', 'id':'c2',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0.175, 'y':0.175,
@ -191,7 +191,7 @@ bricks_start =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'id1', 'id':'d1',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0.175, 'y':0.175,
@ -201,7 +201,7 @@ bricks_start =[{
'yaw':0 'yaw':0
}, },
{ {
'id':'ie1', 'id':'e1',
'rframe':'ia1', 'rframe':'ia1',
'x':0, 'x':0,
'y':0.175, 'y':0.175,
@ -363,8 +363,7 @@ def load_objects():
table_reference_frame="world" table_reference_frame="world"
spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame) spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame)
brick_ents = [] brick_ents = []
for ar in [bricks_start, bricks_end]: for brick in bricks_start:
for brick in ar:
ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation 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 = 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']) 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(): class tf_service():
def __init__(self): def __init__(self):
self._nodes = ['baxter::base', self._nodes = ['baxter::base',
't1::Table'] 't1::Table',
# 'ia1::Brick'] 'a1::Brick',
# 'ia2::Brick', 'a2::Brick',
# 'ia3::Brick', 'a3::Brick',
# 'ib1::Brick', 'b1::Brick',
# 'ib2::Brick', 'b2::Brick',
# 'ic1::Brick', 'c1::Brick',
# 'ic2::Brick', 'c2::Brick',
# 'id1::Brick', 'd1::Brick',
# 'ie1::Brick'] 'e1::Brick']
def _callback(self, data): def _callback(self, data):
# Get index # Get index
for object_name in self._nodes: 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) # print(object_name)
object_idx = data.name.index(object_name) object_idx = data.name.index(object_name)
# Get baxter_base pose and object pose w.r.t. gazebo world # Get baxter_base pose and object pose w.r.t. gazebo world
@ -28,11 +35,7 @@ class tf_service():
br = tf.TransformBroadcaster() br = tf.TransformBroadcaster()
br.sendTransform((object_pose.position.x, object_pose.position.y, object_pose.position.z), 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), (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')
# 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')
def gazebo_link_subscriber(self): def gazebo_link_subscriber(self):