diff --git a/bricks_etc.py b/bricks_etc.py new file mode 100644 index 0000000..0f3865f --- /dev/null +++ b/bricks_etc.py @@ -0,0 +1,213 @@ +from math import radians + +table = { + 'id':'t1', + 'rframe':'world', + 'x':0.85, + 'y':0.20, + 'z':0, + 'roll':0, + 'pitch':0, + 'yaw':radians(90) + } + +bricks_end =[{ + 'id':'ea1', + 'rframe':'t1', + 'x':0.46, + 'y':-0.19, + 'z':0.822, + 'roll':0, + 'pitch':-1.570796, + 'yaw':radians(90) + }, + { + 'id':'ea2', + 'rframe':'ea1', + 'x':0, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ea3', + 'rframe':'ea1', + 'x':0, + 'y':0, + 'z':-0.372, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'eb1', + 'rframe':'ea1', + 'x':0.127, + 'y':0, + 'z':-0.085, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'id':'eb2', + 'rframe':'ea1', + 'x':0.127, + 'y':0, + 'z':-0.287, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'id':'ec1', + 'rframe':'ea1', + 'x':0.254, + 'y':0, + 'z':-0.093, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ec2', + 'rframe':'ea1', + 'x':0.254, + 'y':0, + 'z':-0.287, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'ed1', + 'rframe':'ea1', + 'x':0.381, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':1.57, + 'yaw':0 + }, + { + 'id':'ee1', + 'rframe':'ea1', + 'x':0.508, + 'y':0, + 'z':-0.186, + 'roll':0, + 'pitch':0, + 'yaw':0 + }] + +bricks_start =[{ + 'id':'a1', + 'rframe':'t1', + 'x':-0.419, + 'y':0.134, + 'z':0.770, + 'roll':radians(90), + 'pitch':0, + 'yaw':radians(90) + }, + { + 'id':'a2', + 'rframe':'a1', + 'x':0, + 'y':0, + 'z':0.192, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'a3', + 'rframe':'a1', + 'x':0, + 'y':0, + 'z':0.384, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'b1', + 'rframe':'a1', + 'x':0, + 'y':0.087, + 'z':0, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'b2', + 'rframe':'a1', + 'x':0, + 'y':0.087, + 'z':0.192, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'c1', + 'rframe':'a1', + 'x':0, + 'y':0.087, + 'z':0.384, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'c2', + 'rframe':'a1', + 'x':0, + 'y':0.175, + 'z':0, + 'roll':0, + 'pitch':0, + 'yaw':0 + }, + { + 'id':'d1', + 'rframe':'a1', + 'x':0, + 'y':0.175, + 'z':0.192, + 'roll':0, + 'pitch':0, + 'yaw':0 + }] + # , + # { + # 'id':'e1', + # 'rframe':'a1', + # 'x':0, + # 'y':0.175, + # 'z':0.384, + # 'roll':0, + # 'pitch':0, + # 'yaw':0 + # }] + +def getBuildable(): + return bricks_start + +def getAll(): + return bricks_start+bricks_end+[table] + +def getTable(): + return table + +def getNodes(): + nodes = ['baxter::base', 't1::Table'] + for obj in bricks_start: + name = obj['id'] + node_name = name+'::Brick' + nodes.append(node_name) + return nodes + diff --git a/main.py b/main.py index 3d4e4c0..59c0f3e 100755 --- a/main.py +++ b/main.py @@ -22,202 +22,12 @@ import threading import object_tf_service as ots import object_tf_client as otc import time +import bricks_etc as bet running = True -table = { - 'id':'t1', - 'rframe':'world', - 'x':0.85, - 'y':0.20, - 'z':0, - 'roll':0, - 'pitch':0, - 'yaw':radians(90) - } - -bricks_end =[{ - 'id':'ea1', - 'rframe':'t1', - 'x':0.46, - 'y':-0.19, - 'z':0.822, - 'roll':0, - 'pitch':-1.570796, - 'yaw':radians(90) - }, - { - 'id':'ea2', - 'rframe':'ea1', - 'x':0, - 'y':0, - 'z':-0.186, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'ea3', - 'rframe':'ea1', - 'x':0, - 'y':0, - 'z':-0.372, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'eb1', - 'rframe':'ea1', - 'x':0.127, - 'y':0, - 'z':-0.085, - 'roll':0, - 'pitch':1.57, - 'yaw':0 - }, - { - 'id':'eb2', - 'rframe':'ea1', - 'x':0.127, - 'y':0, - 'z':-0.287, - 'roll':0, - 'pitch':1.57, - 'yaw':0 - }, - { - 'id':'ec1', - 'rframe':'ea1', - 'x':0.254, - 'y':0, - 'z':-0.093, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'ec2', - 'rframe':'ea1', - 'x':0.254, - 'y':0, - 'z':-0.287, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'ed1', - 'rframe':'ea1', - 'x':0.381, - 'y':0, - 'z':-0.186, - 'roll':0, - 'pitch':1.57, - 'yaw':0 - }, - { - 'id':'ee1', - 'rframe':'ea1', - 'x':0.508, - 'y':0, - 'z':-0.186, - 'roll':0, - 'pitch':0, - 'yaw':0 - }] - -bricks_start =[{ - 'id':'a1', - 'rframe':'t1', - 'x':-0.419, - 'y':0.134, - 'z':0.770, - 'roll':radians(90), - 'pitch':0, - 'yaw':radians(90) - }, - { - 'id':'a2', - 'rframe':'a1', - 'x':0, - 'y':0, - 'z':0.192, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'a3', - 'rframe':'a1', - 'x':0, - 'y':0, - 'z':0.384, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'b1', - 'rframe':'a1', - 'x':0, - 'y':0.087, - 'z':0, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'b2', - 'rframe':'a1', - 'x':0, - 'y':0.087, - 'z':0.192, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'c1', - 'rframe':'a1', - 'x':0, - 'y':0.087, - 'z':0.384, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'c2', - 'rframe':'a1', - 'x':0, - 'y':0.175, - 'z':0, - 'roll':0, - 'pitch':0, - 'yaw':0 - }, - { - 'id':'d1', - 'rframe':'a1', - 'x':0, - 'y':0.175, - 'z':0.192, - 'roll':0, - 'pitch':0, - 'yaw':0 - }] - # , - # { - # 'id':'e1', - # 'rframe':'a1', - # 'x':0, - # 'y':0.175, - # 'z':0.384, - # 'roll':0, - # 'pitch':0, - # 'yaw':0 - # }] +bricks = bet.getBuildable() +table = bet.getTable() class PickAndPlace(object): @@ -368,9 +178,8 @@ def tf_service(init=True): def cleanup(): delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) - for group in [[table], bricks_start, bricks_end]: - for obj in group: - delete_model(obj['id']) + for obj in bet.getAll(): + delete_model(obj['id']) def load_objects(): @@ -388,7 +197,7 @@ def load_objects(): table_reference_frame="world" spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame) brick_ents = [] - for brick in bricks_start: + for brick in bricks: 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']) diff --git a/object_tf_service.py b/object_tf_service.py index 4578103..cc54f90 100755 --- a/object_tf_service.py +++ b/object_tf_service.py @@ -2,24 +2,14 @@ import rospy import tf from gazebo_msgs.msg import LinkStates - +import bricks_etc as bet global SIGKILL # Horrible hack SIGKILL = False class tf_service(): def __init__(self): - self._nodes = ['baxter::base', - 't1::Table', - 'a1::Brick', - 'a2::Brick', - 'a3::Brick', - 'b1::Brick', - 'b2::Brick', - 'c1::Brick', - 'c2::Brick', - 'd1::Brick', - 'e1::Brick'] + self._nodes = bet.getNodes() def _callback(self, data): # Get index diff --git a/run b/run index 7974cf3..6bd76c3 100755 --- a/run +++ b/run @@ -1,21 +1,29 @@ #/bin/bash +echo "Entering git directory..." cd RoboticSandbox -echo 1 +echo "Pulling from git..." git pull -echo 2 +echo "Moving main script..." mv main.py ../ -echo 3 +echo "Moving supporting files..." mv object_tf_service.py ../ mv object_tf_client.py ../ -echo 4 +mv bricks_etc.py ../ +echo "Exiting git directory" cd ../ -echo 5 +echo "Deleting precompiled caches" rm object_tf_service.pyc rm object_tf_client.pyc -echo 6 +rm bricks_etc.pyc +echo "Executing main script..." python main.py -echo 7 +echo "Cleaning up..." +rm object_tf_service.pyc +rm object_tf_client.pyc +rm bricks_etc.pyc +echo "Recovering file hierarchy..." mv main.py RoboticSandbox/ mv object_tf_client.py RoboticSandbox/ mv object_tf_service.py RoboticSandbox/ -echo Done +mv bricks_etc.py RoboticSandbox/ +echo "Done!"