Mech-Vision만 사용하여 포즈를 보내기

이 부분에서는 Mech-Vision만 사용하여 포즈를 보내는 Adapter 샘플 프로그램에 대해 자세히 설명하고자 합니다. 이 부분의 내용에는 다음이 포함됩니다.

배경 소개

이 샘플은 크랭크축 텐딩의 응용 시나리오에 관한 것입니다. 카메라는 상자 위의 브래킷에 고정됩니다. Mech-Vision은 사진을 캡처하고 피킹할 수 있는 작업물의 좌표를 출력해 로봇 측에 보냅니다.

이 샘플은 Mech-Vision의 내장 예시 프로젝트 “대형 비 평면 부품”(파일 ‣ 샘플 프로젝트를 보기 ‣ 부품 로드언로드 ‣ 대형 비 평면 부품)을 사용합니다.

이 프로젝트는 3D 모델 매칭 알고리즘을 사용하고 다른 작업물에 대해 다른 모델 파일과 픽 포인트를 설정해야 합니다. 따라서 Mech-Vision에서 파라미터 레시피를 설정해야 하며, 로봇 측에서 사진 캡처 명령을 보낼 때 레시피 번호(작업물 번호)를 설정해야 합니다.

통신 방안

로봇과 Mech-Mind 비전 시리즈 소프트웨어를 설치하는 IPC는 TCP/IP Socket 프로토콜을 사용하여 통신하며 통신 형식은 ASCII 문자열이며 데이터 구분 기호는 영어 쉼표(,)를 사용합니다. 그 중 비전 시리즈 소프트웨어는 통신 서버로, 로봇은 클라이언트로 사용됩니다.

통신 프로세스는 아래 그림과 같습니다.

통신 프로세스의 상세한 설명은 아래와 같습니다.

1. Mech-Center는 로봇이 이미지 캡처 명령 ``P``와 레시피 번호를 보낼 때까지 기다립니다.

2. Mech-Center는 Mech-Vision을 트리거하여 파라미터 레시피를 전환합니다.

3. Mech-Center는 Mech-Vision을 트리거하여 이미지를 캡처하고 대상물을 인식합니다.

4. Mech-Vision이 이미지를 캡처하고 성공적으로 인식한 후 상태 코드와 포즈를 Mech-Center에 반환합니다.

5. Mech-Center는 상태 코드와 포즈를 로봇에 반환합니다.

참고

로봇 피킹를 용이하게 하기 위해 Mech-Center는 피킹할 작업물의 좌표를 로봇 TCP 좌표로 변환합니다.

통신 메시지 형식

자세한 통신 메시지 형식은 아래와 같습니다.

	이미지 캡처 명령	작업물 번호
전송（로봇 -> IPC）	P	정수, 값 범위: 1~100
	상태 코드	포즈(TCP 좌표)
수신（IPC -> 로봇）	정수, 값 범위: 0~4 (0-정상 인식, 1-잘못된 명령 코드, 2-Vision 프로젝트가 등록되지 않음, 3-포즈가 없음, 4-포인트 클라우드가 없음)	x,y,z,a,b,c 형식의 쉼표(,)로 구분된 6개의 부동 소수점 데이터입니다.

참고

응답 메시지의 길이는 고정되어 있습니다. 응답 메시지의 상태 코드가 비정상 코드(1~4)인 경우 비전 포인트 데이터는 0으로 채워집니다.

통신 메시지 샘플

요청 메시지

P,1

정상적인 응답 메시지

0,1994.9217,-192.198,506.4646,-23.5336,-0.2311,173.6517

참고

이 샘플에서 Mech-Vision은 정상적으로 인식하며 반환된 TCP 좌표는 1994.9217,-192.198,506.4646,-23.5336,-0.2311,173.6517입니다.

비정상적인 응답 메시지: 잘못된 명령 코드

1,0,0,0,0,0,0

비정상 응답 메시지: 비전 프로젝트가 등록되지 않음

2,0,0,0,0,0,0

비정상적인 응답 메시지: 포즈가 없음

3,0,0,0,0,0,0

비정상적인 응답 메시지: 포인트 클라우드가 없음

4,0,0,0,0,0,0

프로그래밍 맥락

이 샘플은 프로젝트 목표를 달성하기 위해 다음 그림과 같은 맥락에 따라 Adapter 프로그램을 작성합니다.

위의 그림은 샘플 프로그램의 메시지 처리 논리만을 나열한 것입니다. 다음 부분에서는 샘플 프로그램에 대해 자세히 설명합니다.

샘플 프로그램 상세 소개

참고

Adpater 샘플 프로그램 을 클릭하여 다운로드하십시오.

Python 패키지를 가져오기

Adapter 프로그램이 의존하는 모든 모듈을 도입합니다.

import json
import logging
import math
import sys
from time import sleep
import os

sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..")))
from transforms3d import euler
from interface.adapter import TcpServerAdapter, TcpClientAdapter
from util.transforms import object2tcp

클래스 정의

“TcpServerAdapter” 상위 클래스를 상속하는 “TestAdapter” 하위 클래스를 정의하십시오.

class TestAdapter(TcpServerAdapter):
    vision_project_name = "Large_Non_Planar_Workpieces"
    # vision_project_name = 'Vis-2StationR7-WorkobjectRecognition-L1'
    is_force_real_run = True
    service_name = "test Adapter"

    def __init__(self, address):
        super().__init__(address)
        self.robot_service = None
        self.set_recv_size(1024)

참고

이 샘플은 Adapter 프로그램을 TCP/IP 소켓 통신용 서버로 정의합니다.

명령 수신 및 논리 처리 설정

수신을 받기 위한 처리 논리를 설정합니다(이미지 캡처 및 파라미터 레시피 포함).

# Receive command _create_received_section
def handle_command(self, cmds):
    photo_cmd, *extra_cmds = cmds.decode().split(',')
    recipe = extra_cmds[0]
    # Check command validity _check_cmd_validity_section
    if photo_cmd != 'P':
        self.msg_signal.emit(logging.ERROR, 'Illegal command: {}'.format(photo_cmd))
        self.send(('1' + '' + '').encode())
        return
    # Check whether vision is registered _check_vision_service_section
    if not self.is_vision_started():
        self.msg_signal.emit(logging.ERROR, 'Vision not registered: {}'.format(self.vision_project_name))
        self.send(('2' + '' + '').encode())
        return
    # Change TODO parameter "extra_cmds" according to actual conditions
    sleep(0.1)  # wait for a cycle of getting in Vision
    # _check_vision_result_function_section

    try:
        result = self.select_parameter_group(self.vision_project_name, int(recipe) - 1)
        if result:
            result = result.decode()
            if result.startswith("CV-E0401"):
                self.send(('5' + '' + '').encode())
                return
            elif result.startswith("CV-E0403"):
                self.send(('5' + '' + '').encode())
                return
            raise RuntimeError(result)
    except Exception as e:
        logging.exception('Exception happened when switching model: {}'.format(e))
        self.send(('5' + '' + '').encode())
        return
    self.show_custom_message(logging.INFO, "Switched model for project successfully")

    self.msg_signal.emit(logging.WARNING, 'Started capturing image')
    try:
        self.check_vision_result(json.loads(self.find_vision_pose().decode()))

    except Exception as e:
        self.msg_signal.emit(logging.ERROR, 'Calling project timed out. Please check whether the project is correct: {}'.format(e))
        self.send(('2' + '' + '').encode())

참고

“handle_command” 함수는 TCP/IP 소켓 서버가 수신한 메시지에 대한 처리 인포트로 사용됩니다.

Mech-Vision 비전 결과의 검사를 정의하기

Mech-Vision에서 출력한 비전 결과를 검사하도록 Adapter를 설정합니다.

# Check vision results
def check_vision_result(self, vision_result, at=None):

    noCloudInRoi = vision_result.get('noCloudInRoi', True)
    if noCloudInRoi:
        self.msg_signal.emit(logging.ERROR, 'No point clouds')
        self.send(('4' + '' + '').encode())
        return

    poses = vision_result.get('poses')
    labels = vision_result.get('labels')
    if not poses or not poses[0]:
        self.msg_signal.emit(logging.ERROR, 'No visual points')
        self.send(('3' + '' + '').encode())
        return

    self.send(self.pack_pose(poses, labels).encode())
    self.msg_signal.emit(logging.INFO, 'Sent TCP successfully')

포즈의 출력 형식을 설정하기

비전 포인트의 외부로 출력하는 형식을 설정합니다.

# Pack pose _pack_pose_section
def pack_pose(self, poses, labels, at=None):

    pack_count = min(len(poses), 1)
    msg_body = ''
    for i in range(pack_count):
        pose = poses[i]
        object2tcp(pose)
        t = [p * 1000 for p in pose[:3]]
        r = [math.degrees(p) for p in euler.quat2euler(pose[3:], 'rzyx')]
        p = t + r
        self.msg_signal.emit(logging.INFO, 'Sent pose: {}'.format(p))
        msg_body += ('{:.4f},' * (len(p) - 1) + '{:.4f}').format(*p)

        if i != (pack_count - 1):
            msg_body += ','
    return '{},'.format(0) + msg_body + ''

Adapter를 닫는 작업을 정의하기

Adapter를 닫는 방법을 정의합니다.

def close(self):
    super().close()