OpenMANIPULATOR-X の teleop_keyboard をちょこっと改造

初めに

ROS2 foxy 上で、OpenManipulator-x の teleop_keyboard を改造して、指定した動作を連続的にできるようにします。

基本操作

ROBOTIS OpenMANIPULATOR-X e-manual で説明されているセットアップが完了している前提です。

e-manual 通りにセットアップされていると、以下のフォルダの中に teleop_keyboard.py があるので、エディタで開いて下さい。
~/colcon_ws/src/open_manipulator/open_manipulator_x_teleop/open_manipulator_x_teleop

以下の操作を行い、最後の om_teleop.py に上記の teleop_keyboard.py の内容をコピーします。
1行目：すでに存在しているディレクトリに移動します。
2行目：新たにパッケージ open_manipulator_x_teleop1 を作成し、ノード名を om_teleop で設定します。
3行目:om_teleop.pyがあるフォルダに移動します。
4行目:om_teleop.py をエディタで開きます。

準備操作

cd ~/colcon_ws/src/open_manipulator
ros2 pkg create --build-type ament_python --node-name om_teleop open_manipulator_x_teleop1
cd open_manipulator_x_teleop1/open_manipulator_x_teleop1
gedit om_teleop

最初の方の from ... import ...が並んでいる所の最下行に以下を追加します。

from time import sleep

最後の方の　def main の try: while(rclpy.ok()): の 1 行前に　i=0 を以下のように追加します。さらに、
rclpy.spin_once(om_teleop)の次の行以降を以下のように書き換えます。
if 文の中の命令は、元の teleop_keyboard.py の内容を参考にして、自分の好きな動作に設定します。

    i = 0
    try:
        while(rclpy.ok()):
            rclpy.spin_once(om_teleop)
            i = i + 1
            print (i)
            if i == 2:
                goal_joint_angle[0] = 0.0
                goal_joint_angle[1] = 0.1
                goal_joint_angle[2] = 0.1
                goal_joint_angle[3] = 0.2
                pathtime = 5.0
                om_teleop.send_goal_joint_space(pathtime)
                sleep(pathtime)

            if i == 3:
                goal_joint_angle[0] = 0.0
                goal_joint_angle[1] = 0.0
                goal_joint_angle[2] = 0.0
                goal_joint_angle[3] = 0.0
                pathtime = 5.0
                om_teleop.send_goal_joint_space(pathtime)
                sleep(pathtime)

            if i == 4:
                goal_joint_angle[0] = 0.0
                goal_joint_angle[1] = -1.05
                goal_joint_angle[2] = 0.35
                goal_joint_angle[3] = 0.70
                pathtime = 5.0
                om_teleop.send_goal_joint_space(pathtime)
                sleep(pathtime)

            if i > 5:
                break

            for index in range(0, 7):
                prev_goal_kinematics_pose[index] = goal_kinematics_pose[index]
            for index in range(0, 5):
                prev_goal_joint_angle[index] = goal_joint_angle[index]

om_teleop.pyがあるフォルダの一つ上のフォルダにあるsetup.py の中の entry_points の部分を以下のように修正します。

 entry_points={
        'console_scripts': [
            'om_teleop = open_manipulator_x_teleop1.om_teleop:main'
        ],
    },

実行方法

以下を順番に別々の端末から実行します。最後の rviz は実行すると、PC画面で実機の動きが確認できます（rvizは実行しなくても可）。

1,3 行目は e-manual の記載の通り

ros2 launch open_manipulator_x_controller open_manipulator_x_controller.launch.py
ros2 run open_manipulator_x_teleop1 om_teleop
ros2 launch open_manipulator_x_description open_manipulator_x_rviz.launch.py

参考文献

1. ROBOTIS OpenMANIPULATOR-X e-manual