Exemplo n.º 1
0
def main(args):
    
    global p

    rospy.init_node('art_db_service_tester', anonymous=True)
    
    p = Program()
    p.id = 0
    p.name = "Basic pick&place"
    
    p0 = ProgramItem()
    p0.id = 0
    p0.on_success = 1
    p0.on_failure = 100
    p0.type = ProgramItem.GET_READY
    p.items.append(p0)
    
    p1 = ProgramItem()
    p1.id = 1
    p1.on_success = 2
    p1.on_failure = 100
    p1.type = ProgramItem.WAIT
    p1.spec = ProgramItem.WAIT_FOR_USER
    p.items.append(p1)
    
    p2 = ProgramItem()
    p2.id = 2
    p2.on_success = 3
    p2.on_failure = 100
    p2.type = ProgramItem.MANIP_PICK_PLACE
    p2.spec = ProgramItem.MANIP_TYPE
    p2.object = "profile"
    # TODO p2.pick_polygon
    p2.place_pose.header.frame_id = "marker"
    p2.place_pose.pose.position.x = 0.75
    p2.place_pose.pose.position.y = 0.5
    p.items.append(p2)
    
    p3 = ProgramItem()
    p3.id = 3
    p3.on_success = 4
    p3.on_failure = 100
    p3.type = ProgramItem.WAIT
    p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
    p.items.append(p3)
    
    p4 = ProgramItem()
    p4.id = 4
    p4.on_success = 0
    p4.on_failure = 100
    p4.type = ProgramItem.MANIP_PICK_PLACE
    p4.spec = ProgramItem.MANIP_TYPE
    p4.object = "profile"
    # TODO p4.pick_polygon?
    p4.pick_pose = p2.place_pose
    p4.place_pose.header.frame_id = "marker"
    p4.place_pose.pose.position.x = 0.25
    p4.place_pose.pose.position.y = 0.5
    p.items.append(p4)
    
    rospy.wait_for_service('/art/db/program/store')
    
    try:
        store_program_srv = rospy.ServiceProxy('/art/db/program/store', storeProgram)
        resp = store_program_srv(program=p)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e
        return
Exemplo n.º 2
0
    def test_program(self):

        p = Program()
        p.id = 999
        p.name = "Test pick&place"

        pb = ProgramBlock()
        pb.id = 1  # can't be zero
        pb.name = "First block"
        pb.on_success = 1
        pb.on_failure = 0
        p.blocks.append(pb)

        p0 = ProgramItem()
        p0.id = 1
        p0.on_success = 2
        p0.on_failure = 0
        p0.type = ProgramItem.GET_READY
        pb.items.append(p0)

        p1 = ProgramItem()
        p1.id = 2
        p1.on_success = 3
        p1.on_failure = 0
        p1.type = ProgramItem.WAIT
        p1.spec = ProgramItem.WAIT_FOR_USER
        pb.items.append(p1)

        p2 = ProgramItem()
        p2.id = 3
        p2.on_success = 4
        p2.on_failure = 0
        p2.type = ProgramItem.MANIP_PICK_PLACE
        p2.spec = ProgramItem.MANIP_TYPE
        p2.object = "profile"
        # TODO p2.pick_polygon
        p2.place_pose.header.frame_id = "marker"
        p2.place_pose.pose.position.x = 0.75
        p2.place_pose.pose.position.y = 0.5
        pb.items.append(p2)

        p3 = ProgramItem()
        p3.id = 4
        p3.on_success = 5
        p3.on_failure = 0
        p3.type = ProgramItem.WAIT
        p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
        pb.items.append(p3)

        p4 = ProgramItem()
        p4.id = 5
        p4.on_success = 0
        p4.on_failure = 0
        p4.type = ProgramItem.MANIP_PICK_PLACE
        p4.spec = ProgramItem.MANIP_TYPE
        p4.object = "profile"
        # TODO p4.pick_polygon?
        p4.pick_pose = p2.place_pose
        p4.place_pose.header.frame_id = "marker"
        p4.place_pose.pose.position.x = 0.25
        p4.place_pose.pose.position.y = 0.5
        pb.items.append(p4)

        try:
            resp_store = self.store_program_srv(program=p)
        except rospy.ServiceException:
            pass

        self.assertEquals(resp_store.success, True, "program_store")

        try:
            resp_get = self.get_program_srv(id=999)
        except rospy.ServiceException:
            pass

        self.assertEquals(resp_get.success, True, "program_get")
        self.assertEquals(resp_get.program.id, 999, "program_get")
Exemplo n.º 3
0
    def setUp(self):

        # TODO add more blocks

        self.prog = Program()
        self.ph = ProgramHelper()

        self.prog.id = 666
        self.prog.name = "Basic pick&place"

        pb = ProgramBlock()
        pb.id = 1  # can't be zero
        pb.name = "First block"
        pb.on_success = 1
        pb.on_failure = 0
        self.prog.blocks.append(pb)

        p0 = ProgramItem()
        p0.id = 1
        p0.on_success = 2
        p0.on_failure = 0
        p0.type = ProgramItem.GET_READY
        pb.items.append(p0)

        p1 = ProgramItem()
        p1.id = 2
        p1.on_success = 3
        p1.on_failure = 0
        p1.type = ProgramItem.WAIT
        p1.spec = ProgramItem.WAIT_FOR_USER
        pb.items.append(p1)

        p2 = ProgramItem()
        p2.id = 3
        p2.on_success = 4
        p2.on_failure = 0
        p2.type = ProgramItem.MANIP_PICK_PLACE
        p2.spec = ProgramItem.MANIP_TYPE
        p2.object = "profile"
        p2.place_pose.header.frame_id = "marker"
        p2.place_pose.pose.position.x = 0.75
        p2.place_pose.pose.position.y = 0.5
        pb.items.append(p2)

        p3 = ProgramItem()
        p3.id = 4
        p3.on_success = 5
        p3.on_failure = 0
        p3.type = ProgramItem.WAIT
        p3.spec = ProgramItem.WAIT_UNTIL_USER_FINISHES
        pb.items.append(p3)

        p4 = ProgramItem()
        p4.id = 5
        p4.on_success = 0
        p4.on_failure = 0
        p4.type = ProgramItem.MANIP_PICK_PLACE
        p4.spec = ProgramItem.MANIP_TYPE
        p4.object = "profile"
        p4.pick_pose = p2.place_pose
        p4.place_pose.header.frame_id = "marker"
        p4.place_pose.pose.position.x = 0.25
        p4.place_pose.pose.position.y = 0.5
        pb.items.append(p4)