class MasterController(object):
box_pose = {}
box_pose["PURPLE"] = Pose(
position=Point(x=0.31, y=0.1445, z=0.1), orientation=Quaternion(x=0, y=math.pi / 4, z=0, w=0)
)
box_pose["ORANGE"] = Pose(
position=Point(x=0.31, y=0.1445 - 36 * 0.0254, z=0.1), orientation=Quaternion(x=0, y=math.pi / 4, z=0, w=0)
)
box_pose["GREEN"] = Pose(
position=Point(x=0.31, y=0.1445 - 27 * 0.0254, z=0.1), orientation=Quaternion(x=0, y=math.pi / 4, z=0, w=0)
)
def __init__(self, setconfig=False):
rospy.init_node("senior_design")
self.right_camera = ImageReceiver("right_hand_camera")
self.left_camera = ImageReceiver("left_hand_camera")
self.head_camera = ImageReceiver("head_camera")
self.left_camera.disableCamera()
self.head_camera.disableCamera()
self.right_camera.enableCamera()
distortion, camera_matrix = self.right_camera.getIntrinsics()
self.move = MoveController("right")
self.image_processor = ImageProcessor(self.move.home_pose, camera_matrix, distortion)
self.block_list = {"PURPLE": [], "ORANGE": [], "GREEN": []}
# This is all for loading / calculating /saving the configuration information i.e. box location and table height
if setconfig:
self.move.update_table_height()
f = open("config.txt", "w")
outStr = "TABLE_HEIGHT:" + str(self.move.table_height) + "\n"
f.write(outStr)
self.find_blocks()
blackoutval = -100
for color in self.block_list:
maxxvalue = -100
for block in self.block_list[color]:
if block.pose.position.x > maxxvalue:
maxxvalue = block.pose.position.x
self.box_pose[color] = block.pose
if maxxvalue > blackoutval:
blackoutval = maxxvalue
blackoutblock = block
if maxxvalue == -100:
print "ERROR ALL THREE COLORS NOT DETECTED MISSING " + color + " BLOCK"
sys.exit()
else:
outStr = (
color
+ ":"
+ str(self.box_pose[color].position.x - 0.08)
+ ","
+ str(self.box_pose[color].position.y)
+ "\n"
)
f.write(outStr)
coord = blackoutblock.coord
self.image_processor.blackout = min(coord[0][1], coord[2][1])
outStr = "BLACKOUT:" + str(self.image_processor.blackout) + "\n"
f.write(outStr)
f.close()
else:
# Load config file
try:
f = open("config.txt", "r")
newLine = f.readline()
while newLine != "":
newLine = newLine.split(":")
if len(newLine) == 2:
if newLine[0] == "TABLE_HEIGHT":
self.move.table_height = float(newLine[1])
self.image_processor.table_height = float(newLine[1])
self.update_home_pose(self.move.home_pose)
if newLine[0] == "BLACKOUT":
print int(newLine[1])
self.image_processor.blackout = int(newLine[1])
if self.box_pose.get(newLine[0], None) != None:
coords = newLine[1].split(",")
if len(coords) == 2:
self.box_pose[newLine[0]].position.x = float(coords[0])
self.box_pose[newLine[0]].position.y = float(coords[1])
else:
print "INVALID CONFIG FILE SHOULD BE COLOR:X,Y" + str(newLine)
sys.exit()
newLine = f.readline()
f.close()
except:
print "MISSING config.txt PLEASE RUN WITH -c FLAG"
print "PROCEEDING WITH DEFAULT TABLE HEIGHT AND NO IMAGE BLACKOUT VALUE"
def update_home_pose(self, pose):
self.image_processor.update_home_pose(pose)
self.move.home_pose = pose
def get_home_image(self):
self.move.move_to_home()
rospy.sleep(0.1)
image = self.right_camera.getImage()
self.image_processor.setImage(image)
def find_blocks(self):
self.get_home_image()
self.block_list["PURPLE"] = self.image_processor.findBlock("PURPLE")
# print "Found " + str(len(self.block_list['PURPLE'])) + " purple blocks"
self.block_list["ORANGE"] = self.image_processor.findBlock("ORANGE")
# print "Found " + str(len(self.block_list['ORANGE'])) + " orange blocks"
self.block_list["GREEN"] = self.image_processor.findBlock("GREEN")
# print "Found " + str(len(self.block_list['GREEN'])) + " green blocks"
def are_blocks_near(self, block):
xthresh = 0
ythresh = 0.074 # ~3 inches
xpose = block.pose.position.x
ypose = block.pose.position.y
isclose = False
# print "Current block:________" #debug code
# print block.pose.position #debug code
# check for close blue blocks
for color in self.block_list:
for block2 in self.block_list[color]:
# print "Blue block at:" #debug code
# print block2.pose.position #debug code
if (xpose != block2.pose.position.x) and (ypose != block2.pose.position.y):
if math.fabs(xpose - block2.pose.position.x) < xthresh:
print "Block at "
print block.pose
print "is close to blue block in the y direction."
isclose = True
if math.fabs(ypose - block2.pose.position.y) < ythresh:
print "Block at "
print block.pose
print "is close to blue block in the y direction."
isclose = True
return isclose
def get_blocks(self, trials=2, miss_per_trial=2):
result = 0
for i in range(trials):
self.find_blocks()
num_missed = 0
for color in self.block_list:
for block in self.block_list[color]:
result = self.move.pick_at_pose(block.pose)
# result == 0 means block pose was valid, if it wasn't go to the next block
if result == 0:
self.move.raise_up(block.pose)
if not self.move.gripper.gripping():
self.move.gripper.open(block=False)
num_missed += 1
if num_missed == miss_per_trial:
break
else:
self.move.drop_at_pose(self.box_pose[color])
if num_missed == miss_per_trial:
# want to break two loops when the gripper doesn't grip
break
if num_missed == 0:
# Would mean that all blocks were found in the picture
return num_missed
return num_missed