Update
This commit is contained in:
Max Hunt
parent
93af96ded9
commit
d1dd5bc1b6
213
bricks_etc.py
Normal file
213
bricks_etc.py
Normal file
@ -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
|
||||
|
||||
201
main.py
201
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,8 +178,7 @@ 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:
|
||||
for obj in bet.getAll():
|
||||
delete_model(obj['id'])
|
||||
|
||||
|
||||
@ -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'])
|
||||
|
||||
@ -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
|
||||
|
||||
24
run
24
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!"
|
||||
|
||||
Loading…
Reference in New Issue
Block a user