Meta(Oculus)Quest で立体視しながら、コントローラーでロボットハンドの操作:ソースコード:subscriber_member_function.py

ROS2のメイン
MinimalSubscriber__init__初期化
__del__デストラクタ:破壊時に呼び出される
listener_callback処理のメイン
# Copyright 2016 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# Copyright 2022 robot-creation-diary.com
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# チュートリアルの内容から、HMDのオイラー角を受け取りそれに合わせて
# サーボモータを制御するように変更してある
# 使用機材:サーボモータ:Miuzei MS18、サーボモータコントローラ:20軸サーボモータコントローラ WR-MSXX

import rclpy
from rclpy.node import Node
from cpp_pubsub.msg import OculusMsg
import serial
import copy
import numpy as np
from hmdcontroller_sub.robot_support import RobotSupport as RobotSupport
from hmdcontroller_sub.serial_support import SerialSuppot as SerialSuppot


class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            OculusMsg,
            'oculus_topic',
            self.listener_callback,
            10)

        self.subscription  # prevent unused variable warning
        # Comportを開く
        self.serial_support = SerialSuppot("com4", 115200)

        self.pre_left_arm_list = np.array([0] * 7)
        self.pre_right_arm_list = np.array([0] * 7)
        self.left_robot_support = RobotSupport()
        self.right_robot_support = RobotSupport()

        # 首用Comportを開く
        self.serial_support.open_neck_comport("COM5", 38400)

        # デバッグ用
        self.button_pushed = False
        self.button_push_count = 0
        self.hmd_data_error = False

    def __del__(self):
        # comportを閉じる
        pass

    def listener_callback(self, msg):

        # byteのままだと単純整数比較ができないので整数に直す

        a_button = int.from_bytes(msg.a_button, 'little')
        b_button = int.from_bytes(msg.b_button, 'little')
        x_button = int.from_bytes(msg.x_button, 'little')
        y_button = int.from_bytes(msg.y_button, 'little')

        # 座標系を合わせる
        hmd_position = copy.deepcopy(msg.hmd_position)
        head_position = copy.deepcopy(msg.head_position)
        l_controller_position = copy.deepcopy(msg.l_controller_position)
        r_controller_position = copy.deepcopy(msg.r_controller_position)
        l_controller_rotation = copy.deepcopy(msg.l_controller_rotation)
        r_controller_rotation = copy.deepcopy(msg.r_controller_rotation)
        head_position[2] *= -1
        hmd_position[2] *= -1
        l_controller_position[2] *= -1
        r_controller_position[2] *= -1
        l_controller_rotation[1] *= -1
        l_controller_rotation[2] *= -1
        r_controller_rotation[1] *= -1
        r_controller_rotation[2] *= -1

        if (self.right_robot_support.check_float_axis(
                                        [0, 0, 0],
                                        hmd_position) and
            self.right_robot_support.check_float_axis(
                                        [0, 0, 0],
                                        l_controller_position) and
            self.right_robot_support.check_float_axis(
                [0, 0, 0],
                r_controller_position)):
            if self.hmd_data_error is False:
                print("HMD data error.")
                self.hmd_data_error = True
            self.serial_support.read_and_print()  # シリアルからのデータを消化しておく
            return
        self.hmd_data_error = False
        # キャリブレーション
        if (1 == b_button):
            # この分岐はデバッグ用
            if (self.right_robot_support.get_calibration_stage() < 1):
                print("---b_button---")
                print("msg.head_euler", msg.head_euler)
                print("head_position", head_position)
                print("r_controller_position", r_controller_position)
                self.right_robot_support.calibrate_hand_positon(
                                            head_position,
                                            r_controller_position,
                                            None)
                self.right_robot_support.calibrate_head_euler(msg.head_euler)

                print(" rsr.calibrate_hand_positon(",
                      head_position,
                      ",",
                      r_controller_position,
                      ",None)")
            elif (1 < self.right_robot_support.get_calibration_stage()):
                self.right_robot_support.set_calibration_stage(3)

        if (1 == a_button):
            # この分岐はデバッグ用
            if (self.right_robot_support.get_calibration_stage() < 2):
                print("---a_button---")
                print("hmd rsr.calibrate_hand_positon(",
                      hmd_position,
                      ", None, ",
                      r_controller_position,
                      ")")
                print(" rsr.calibrate_hand_positon(",
                      head_position,
                      ", None, ",
                      r_controller_position,
                      ")")
                print(" rsr.calibrate_hand_quaternion(",
                      r_controller_rotation,
                      ")")
                self.right_robot_support.calibrate_hand_positon(
                                            head_position,
                                            None,
                                            r_controller_position)

                self.right_robot_support.calibrate_head_euler(msg.head_euler)

                self.right_robot_support.calibrate_hand_quaternion(
                                            r_controller_rotation)
            else:
                self.right_robot_support.set_calibration_stage(3)

        if (1 == y_button):
            # この分岐はデバッグ用
            if (self.left_robot_support.get_calibration_stage() < 1):
                print("---y_button---")
                print(" rsl.calibrate_hand_positon(",
                      head_position,
                      ",",
                      l_controller_position,
                      ",None)")
                self.left_robot_support.calibrate_hand_positon(
                                            head_position,
                                            l_controller_position,
                                            None)
            elif (1 < self.left_robot_support.get_calibration_stage()):
                self.left_robot_support.set_calibration_stage(3)

        if (1 == x_button):
            # この分岐はデバッグ用
            if (self.left_robot_support.get_calibration_stage() < 2):
                print("---x_button---")
                print("hmd rsr.calibrate_hand_positon(",
                      hmd_position,
                      ", None, ",
                      l_controller_position,
                      ")")
                print(" rsl.calibrate_hand_positon(",
                      head_position,
                      ", None, ",
                      l_controller_position,
                      ")")
                print(" rsl.calibrate_hand_quaternion(",
                      l_controller_rotation,
                      ")")
                self.left_robot_support.calibrate_hand_positon(
                                            head_position,
                                            None,
                                            l_controller_position)
                self.left_robot_support.calibrate_hand_quaternion(
                                            l_controller_rotation)
            else:
                self.left_robot_support.set_calibration_stage(3)

        # デバッグ用データ出力
        if ((2 < self.left_robot_support.get_calibration_stage()) and
                (2 < self.right_robot_support.get_calibration_stage())):
            if ((1 == a_button) or (1 == b_button) or
                    (1 == x_button) or (1 == y_button)):
                if self.button_pushed is False:
                    self.button_pushed = True
                    self.button_push_count += 1
                    print("---button---", self.button_push_count)
                    print("head_positkon", head_position)
                    print("hmd_position", hmd_position)
                    print(" r_nparray = rsr.seven_link_ik(",
                          r_controller_position,
                          ",",
                          r_controller_rotation,
                          ")")
                    print(" l_nparray = rsl.seven_link_ik(",
                          l_controller_position,
                          ",",
                          l_controller_rotation,
                          ")")
            else:
                self.button_pushed = False
        # デバッグ用 リターン
        # return

        # キャリブレーション結果を持ち出で
        # 初期値を脱力状態にする。
        left_arm_list = [240] * 7
        right_arm_list = [240] * 7
        left_hand = 240
        right_hand = 240
        # ボタン欧化5の倍数の時脱力
        if (0 != self.left_robot_support.get_calibration_stage() % 5):
            if (1 < self.left_robot_support.get_calibration_stage()):
                if self.left_robot_support.check_float_list(
                        [0, 0, 0, 0],
                        l_controller_rotation) is False:
                    left_arm_list = list(self.left_robot_support.seven_link_ik(
                                            l_controller_position,
                                            l_controller_rotation))
                    left_hand = (msg.l_triger * 120.0 + 100.0) * np.pi / 180.0
                    # nanの時前の角度を保つ
                    for i in range(7):
                        if np.isnan(left_arm_list[i]):
                            left_arm_list[i] = copy.deepcopy(
                                                self.pre_left_arm_list[i])
                            print(" l_nparray = rsl.seven_link_ik(",
                                  l_controller_position,
                                  ",",
                                  l_controller_rotation,
                                  ")")
                        else:
                            self.pre_left_arm_list[i] =\
                                copy.deepcopy(left_arm_list[i])

            if (1 < self.right_robot_support.get_calibration_stage()):
                if self.left_robot_support.check_float_list(
                        [0, 0, 0, 0],
                        r_controller_rotation) is False:
                    right_arm_list =\
                        list(self.right_robot_support.seven_link_ik(
                             r_controller_position,
                             r_controller_rotation))
                    right_hand = (msg.r_triger * 120.0 + 100.0) * np.pi / 180

                    # nanの時前の角度を保つ
                    for i in range(7):
                        if np.isnan(right_arm_list[i]):
                            right_arm_list[i] =\
                                copy.deepcopy(self.pre_right_arm_list[i])
                            print(" r_nparray = rsr.seven_link_ik(",
                                  r_controller_position,
                                  ",",
                                  r_controller_rotation,
                                  ")")
                        else:
                            self.pre_right_arm_list[i] =\
                                copy.deepcopy(right_arm_list[i])

        if ((1 < self.right_robot_support.get_calibration_stage()) and
                (1 < self.left_robot_support.get_calibration_stage())):
            self.serial_support.move_servo(
                left_arm_list, left_hand,
                right_arm_list, right_hand)

            ce = self.right_robot_support.get_calibration_head_euler([msg.head_euler[0], msg.head_euler[1], 0])
            self.serial_support.move_neck_servo(
                self.right_robot_support.euler_to_servo(
                    ce[0], -45, False),
                self.right_robot_support.euler_to_servo(
                    ce[1] * -1, 0, True))


def main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()