Ejemplo n.º 1
0
 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
Ejemplo n.º 2
0
 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)
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
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")
Ejemplo n.º 5
0
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()