def on_key_press(self, pressed, modifiers):
"""
Defines the logic to do when a key is pressed
:param pressed: The key that was pressed
:param modifiers: The modifiers pressed alongside the key
:return: None
"""
if pressed == key.ESCAPE:
self.save_world()
self.close()
log.INFO("MineGlet was closed!")
elif pressed == key.E:
self.mouse_lock = not self.mouse_lock
def __init__(self):
self.num_of_grippers = rospy.get_param("~num_grippers", 1)
# make sure 1 gripper is detected
if self.num_of_grippers != 1:
LOG.ERROR("Incorrect number of grippers detected: {}".format(
self.num_of_grippers))
return
else:
LOG.INFO("Correct number of gripper detected.")
rospy.Subscriber("/gripper/stat",
GripperStat,
self.update_gripper_stat,
queue_size=10)
self.gripper_publisher = rospy.Publisher('/gripper/cmd',
GripperCmd,
queue_size=10)
self.gripper_stat = GripperStat()
self.gripper_cmd = GripperCmd()
# set the gripper force and speed to default value
self.set_gripper_force(100.0)
self.set_gripper_speed(0.02)
self.r = rospy.Rate(1)
texture = pyglet.image.load(filename).get_texture()
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST)
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST)
return pyglet.graphics.TextureGroup(texture)
TEXTURE_DIRECTORY = "textures/"
# Raw textures
GRASS_TOP = get_texture(TEXTURE_DIRECTORY + "GRASS_TOP.png")
GRASS_SIDE = get_texture(TEXTURE_DIRECTORY + "GRASS_SIDE.png")
DIRT = get_texture(TEXTURE_DIRECTORY + "DIRT.png")
SAND = get_texture(TEXTURE_DIRECTORY + "SAND.png")
BRICK = get_texture(TEXTURE_DIRECTORY + "BRICK.png")
STONE = get_texture(TEXTURE_DIRECTORY + "STONE.png")
AIR = get_texture(TEXTURE_DIRECTORY + "AIR.png")
log.INFO("Created textures using the get_texture method for every block")
class Texture:
def __init__(self, top, side, bottom):
self.top = top
self.side = side
self.bottom = bottom
class Textures:
def __init__(self):
self.GRASS = Texture(GRASS_TOP, GRASS_SIDE, DIRT)
self.DIRT = Texture(DIRT, DIRT, DIRT)
self.SAND = Texture(SAND, SAND, SAND)
self.BRICK = Texture(BRICK, BRICK, BRICK)
def execute_plan(acHan, json_plan_file="plan.json"):
if os.path.exists(json_plan_file):
with open(json_plan_file, "r") as f:
plan_json = json.load(f)
# initialize the parser with the json Data received from the socket
parser = ActionParser(plan_json)
actionType = parser.getType()
# check ROS alive
if actionType == 'CheckROSLive':
LOG.INFO("Checking ROS alive...\n")
save_reply(actionType, True)
elif actionType == 'launchROS':
LOG.INFO("Launch ROS scripts...\n")
save_reply(actionType, True)
# execute a plan
elif actionType == 'ExecutePlan':
LOG.INFO("Executing the therbligs plan...\n")
# start VREP simulation
#if acHan.simulator != None:
# acHan.simulator.start_simulation()
# iterate through different tasks
for i, task in enumerate(parser.getTasks()):
LOG.INFO("Starting to execute ", task['name'],'...')
# iterate list of therbligs in the current task
print ("sizesizesize",len(parser.getTherbligs(i)))
for j, therblig in enumerate(parser.getTherbligs(i)):
LOG.INFO("Next therblig: ", parser.getTherbligName(therblig))
if parser.getTherbligName(therblig) == "Transport Empty":
LOG.INFO("Object info: ", "XYZ-position:", parser.getXYZPosition(therblig), "\nOrientation:", parser.getOrientation(therblig))
# Call Transport Empty API from mico_planner
pose_target = createTarget(parser.getXYZPosition(therblig), parser.getOrientation(therblig))
if pose_target == None:
continue
ret = acHan.Transport_Empty(pose_target)
if not ret:
LOG.ERROR("Transport Empty failed")
elif parser.getTherbligName(therblig) == "Grasp":
LOG.INFO("Grasping object: ", parser.getObjectName(therblig))
# Call grasp API from mico_planner
# open the gripper
ret = acHan.Set_Hand_Openness(0.085)
if not ret:
LOG.ERROR("Grasp object failed")
# close the gripper
ret = acHan.Set_Hand_Openness(0.025)
if not ret:
LOG.ERROR("Grasp object failed")
elif parser.getTherbligName(therblig) == "Transport Loaded":
LOG.INFO("Object info: ", "XYZ-position:", parser.getXYZPosition(therblig), "\nOrientation:", parser.getOrientation(therblig))
# Call Transport Loaded API from mico_planner
pose_target = createTarget(parser.getXYZPosition(therblig), parser.getOrientation(therblig))
if pose_target == None:
continue
ret = acHan.Transport_Loaded(pose_target)
if not ret:
LOG.ERROR("Transport Loaded failed")
elif parser.getTherbligName(therblig) == "Release Load":
LOG.INFO("Releasing object: ", parser.getObjectName(therblig))
# release the object
ret = acHan.Set_Hand_Openness(0.085)
if not ret:
LOG.ERROR("Release object failed")
elif parser.getTherbligName(therblig) == "Rest":
LOG.INFO("Object info: ", "XYZ-position:", parser.getXYZPosition(therblig), "\nOrientation:", parser.getOrientation(therblig))
# Call Rest API from mico_planner
pose_target = createTarget(parser.getXYZPosition(therblig), parser.getOrientation(therblig))
if pose_target == None:
continue
ret = acHan.Rest(pose_target)
if not ret:
LOG.ERROR("Rest failed")
elif parser.getTherbligName(therblig) == "Hold":
duration = (int)(parser.getHoldDuration(therblig))
LOG.INFO("Hold for {} seconds".format(duration))
# Call Hold API from mico_planner
ret = acHan.Hold(pose_target)
if not ret:
LOG.ERROR("Hold failed")
# end VREP simulation
if acHan.simulator != None:
acHan.simulator.end_simulation()
save_reply(actionType, True)
# get position
elif actionType == 'GetPosition':
LOG.INFO("Getting the position...\n")
currPose = acHan.Read_Position()
save_reply(actionType, True)
# exit ros
elif actionType == 'Exit':
LOG.INFO("Exiting...\n")
save_reply(actionType, True)
loop = False
end()
elif actionType == None:
LOG.INFO('USAGE ERROR: Invalid Action Type\n')
else:
LOG.ERROR("Thebligs json does not exists")
def main(port=9999, sim_flag=False):
address = ("", port)
httpd = HTTPServer(address, mico_server_handler)
LOG.INFO("Starting python HTTP server at port {}".format(port))
# start the HTTP server
httpd.serve_forever()