3 ۖ^ @sddlTGdddeZdS))*cs`eZdZfddZfddZGdddeZGdddeZGd d d eZd d Z Z S) GroundTestcstjddddS)NT)viewer_crosshairsparticle_viewer)super__init__)self) __class__./GroundTest.pyrszGroundTest.__init__cstjdtj_dS)NT)rstartrobotcameracolor_image_enabled)r)r r r r s zGroundTest.startcseZdZdfdd ZZS)zGroundTest.ProjectToGroundNcs:tj|d }|jjj|}td|d|dfdS) Ni@z&Camera center point is at %4.1f, %4.1frd@^@)rr)rr r kineproject_to_groundprint)revent camera_centerpoint)r r r r s z GroundTest.ProjectToGround.start)N)__name__ __module__ __qualname__r __classcell__r r )r r ProjectToGround srcseZdZdfdd ZZS)zGroundTest.GrabPatchNcshtj|tj|jjjj}|ddddddf|j_ t j |jj t j }t j d|t jddS)Ns}patchr)rr nparrayr world latest_image raw_imageparentr$cv2cvtColor COLOR_RGB2BGRimshowwaitKey)rrimgZpatch2)r r r r s   zGroundTest.GrabPatch.start)N)rrrr rr r )r r GrabPatchsr1cseZdZdfdd ZZS)zGroundTest.MatchPatchNcstj|tj|jjjj}|ddddddf}||jj j dj }t d|t j|t j}t jd|t jddS) Nr r!r"r#rz Match error =sampler)rr r%r&r r'r(r)r*r$flattensumrr+r,r-r.r/)rrr0r2diffZsample2)r r r r s   zGroundTest.MatchPatch.start)N)rrrr rr r )r r MatchPatchsr6c Cstjdj|}|jjdj|}|jjdj|}|jjdj|}tdjd}|j|j|tdjd}|j|j|td jd }|j|j|t jd }|j|j|t jd } | j|j|t jd } | j|j||S)a2 dispatch: StateNode() dispatch =TM('p')=> project dispatch =TM('g')=> grab dispatch =TM('m')=> match project: self.ProjectToGround() =N=> dispatch grab: self.GrabPatch() =N=> dispatch match: self.MatchPatch() =N=> dispatch dispatchprojectgrabmatchp textmsgtrans1g textmsgtrans2m textmsgtrans3 nulltrans1 nulltrans2 nulltrans3) StateNodeset_name set_parentrr1r6 TextMsgTrans add_sourcesadd_destinations NullTrans) rr7r8r9r:r<r>r@rArBrCr r r setup&s"   zGroundTest.setup) rrrrr rDrr1r6rKrr r )r r rs     rN) cozmo_fsmStateMachineProgramrr r r r s