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 21:22:06 +00:00
parent 93af96ded9
commit d1dd5bc1b6
4 changed files with 237 additions and 217 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

213
bricks_etc.py Normal file
View 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
View File

@ -22,202 +22,12 @@ import threading
import object_tf_service as ots import object_tf_service as ots
import object_tf_client as otc import object_tf_client as otc
import time import time
import bricks_etc as bet
running = True running = True
table = { bricks = bet.getBuildable()
'id':'t1', table = bet.getTable()
'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
# }]
class PickAndPlace(object): class PickAndPlace(object):
@ -368,8 +178,7 @@ def tf_service(init=True):
def cleanup(): def cleanup():
delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
for group in [[table], bricks_start, bricks_end]: for obj in bet.getAll():
for obj in group:
delete_model(obj['id']) delete_model(obj['id'])
@ -388,7 +197,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 brick in bricks_start: for brick in bricks:
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'])

14
object_tf_service.py
View File

@ -2,24 +2,14 @@
import rospy import rospy
import tf import tf
from gazebo_msgs.msg import LinkStates from gazebo_msgs.msg import LinkStates
import bricks_etc as bet
global SIGKILL # Horrible hack global SIGKILL # Horrible hack
SIGKILL = False SIGKILL = False
class tf_service(): class tf_service():
def __init__(self): def __init__(self):
self._nodes = ['baxter::base', self._nodes = bet.getNodes()
't1::Table',
'a1::Brick',
'a2::Brick',
'a3::Brick',
'b1::Brick',
'b2::Brick',
'c1::Brick',
'c2::Brick',
'd1::Brick',
'e1::Brick']
def _callback(self, data): def _callback(self, data):
# Get index # Get index

24
run
View File

@ -1,21 +1,29 @@
#/bin/bash #/bin/bash
echo "Entering git directory..."
cd RoboticSandbox cd RoboticSandbox
echo 1 echo "Pulling from git..."
git pull git pull
echo 2 echo "Moving main script..."
mv main.py ../ mv main.py ../
echo 3 echo "Moving supporting files..."
mv object_tf_service.py ../ mv object_tf_service.py ../
mv object_tf_client.py ../ mv object_tf_client.py ../
echo 4 mv bricks_etc.py ../
echo "Exiting git directory"
cd ../ cd ../
echo 5 echo "Deleting precompiled caches"
rm object_tf_service.pyc rm object_tf_service.pyc
rm object_tf_client.pyc rm object_tf_client.pyc
echo 6 rm bricks_etc.pyc
echo "Executing main script..."
python main.py 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 main.py RoboticSandbox/
mv object_tf_client.py RoboticSandbox/ mv object_tf_client.py RoboticSandbox/
mv object_tf_service.py RoboticSandbox/ mv object_tf_service.py RoboticSandbox/
echo Done mv bricks_etc.py RoboticSandbox/
echo "Done!"