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