Mech-Vision만 사용하여 비전 포즈를 전송하기
이 부분에서는 Mech-Vision만 사용하여 비전 포즈를 전송하는 Adapter 샘플 프로그램에 대해 자세히 소개하겠습니다.
배경 소개
이 샘플은 크랭크 축 텐딩의 응용 시나리오에 관한 것입니다. 카메라는 빈 위의 브래킷에 고정되며 Mech-Vision은 이미지를 캡처하고 피킹할 수 있는 작업물의 좌표를 출력해 로봇 측에 보냅니다.
이 샘플은 Mech-Vision의 내장 샘플 프로젝트 “대형 비 평면 부품”(
)을 사용합니다.이 프로젝트는 3D 모델 매칭 알고리즘을 사용하며 서로 다른 공작물에 대해 서로 다른 모델 파일과 픽 포인트를 설정해야 합니다. 따라서 Mech-Vision에서 파라미터 레시피를 설정해야 하고 레시피 번호(공작물 번호)는 로봇 측에서 이미지 캡처 명령을 보낼 때 설정합니다.
![sample1](_images/sample1.png)
통신 방안
로봇과 Mech-Mind Robotics 비전 소프트웨어 시스템을 설치하는 IPC는 TCP/IP Socket 프로토콜을 사용하여 통신하며 통신 형식은 ASCII 문자열이며 데이터 구분 기호는 영어 쉼표(,)를 사용합니다.
그 중 비전 시스템은 통신 서버로, 로봇은 클라이언트로 사용됩니다.
통신 프로세스는 아래 그림과 같습니다.
![adapter sample1 interaction](_images/adapter_sample1_interaction.png)
통신 프로세스의 상세한 설명은 아래와 같습니다:
-
Mech-Center는 로봇이 이미지 캡처 명령 `P`와 레시피 번호를 보낼 때까지 기다립니다.
-
Mech-Center는 Mech-Vision을 트리거하여 파라미터 레시피를 전환합니다.
-
Mech-Center는 Mech-Vision을 트리거하여 이미지를 캡처합니다.
-
이미지 캡처와 물체 인식이 성공하면 Mech-Vision은 상태 코드와 비전 포즈를 Mech-Center로 전송합니다.
-
Mech-Center는 상태 코드와 비전 포즈를 로봇으로 전송합니다.
로봇 피킹를 용이하게 하기 위해 Mech-Center는 피킹할 작업물의 포즈를 로봇 TCP 포즈로 변환합니다. |
통신 메시지 형식
자세한 통신 메시지 형식은 아래와 같습니다.
요청 명령 | 공작물 번호 | |
---|---|---|
전송(로봇→ IPC) |
|
정수, 값 범위: 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 프로그램을 작성합니다.
![Unnamed image](_images/adapter_sample1_guidelines.png)
위의 그림은 샘플 프로그램의 메시지 처리 논리만을 나열한 것입니다. 다음으로 샘플 프로그램에 대해 자세히 설명하겠습니다.
샘플 프로그램 상세 소개
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()