Mech-Visionを使用して視覚位置姿勢を送信する

本節では、Mech-Visionのみを使用してし視覚位置姿勢を送信するAdapterサンプルプログラムについて詳しく説明していきます。主に以下の内容が含まれます。

背景の紹介

本例は、クランクシャフト供給の応用シーンに適しています。カメラは箱の上方のスタンドに取り付けられ、Mech-Visionは画像を撮影して把持可能な部品の座標をロボット側に出力します。

本例では、Mech-Visionに組み込みの「大型の非平面形状部品」サンプルプロジェクトを使用して説明していきます。Mech-Visionで ファイル ‣ サンプルプロジェクトを閲覧 ‣ 部品のロード・アンロード をクリックして、このプロジェクトが表示されます。

このプロジェクトは3Dモデルマッチングのアルゴリズムを使用し、異なるワークに対して異なるモデルファイルと把持位置姿勢を設定する必要があります。したがって、Mech-Visionでパラメータレシピを設定する必要があり、ロボット側から画像撮影コマンドを送信する際にレシピ番号（ワーク番号）を設定します。レシピ番号の設定や表示方法については、 パラメータレシピ をご参照ください。

通信ソリューション

ロボットは、TCP/IP Socketプロトコルを使用してMech-Mindビジョンシリーズソフトウェアがインストールされた産業用PCと通信します。通信フォーマットは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座標に変換します。

通信メッセージのフォーマット

詳細は下記の通りです。

	画像撮影コマンド	部品番号
送信（ロボット -> 産業用PC）	P	「1~100」範囲にある整数です
	ステータスコード	視覚位置姿勢（TCP座標）
受信（産業用PC -> ロボット）	「0~4」範囲内にある整数です。0-正常認識；1-エラーのコマンドコード；2-Mech-Visionプロジェクトが登録されない；3-視覚位置姿勢なし；4-点群なし	カンマ「,」で区切られた6つの浮動小数点データ (x,y,z,a,b,cの形式)

注釈

応答メッセージの長さは固定です。応答メッセージのステータスコードが異常コード（1~4）の場合、視覚位置姿勢データは0で埋める必要があります。

通信メッセージの例

リクエストメッセージ

P,1

通常応答時のメッセージ

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

注釈

本例では、Mech-Visionは正常に認識し、「1994.9217,-192.198,506.4646,-23.5336,-0.2311,173.6517」のTCP座標を返します。

異常応答時のメッセージ：エラーのコマンドコード

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

異常応答時のメッセージ：Mech-Visionプロジェクトが登録されない

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 Socket通信用のサーバーとして定義します。

受信コマンドと処理ロジックを設定する

リクエスト（画像撮影コマンド、パラメータレシピ含む）を受け取る処理ロジックを設定します。

# 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 Socketサーバーがメッセージを受信するための処理エントリとして使用されます。

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()