# 自定義 `teleop-keyboard`

1. 創建 ros 包

catkin_create_pkg my_motor_control rospy std_msgs

2. 創建節點
在 src 在新增`motor_control_node.py`

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
import tty, termios
import modbus_tk
import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp

def stop():
    logger.info(master_2.execute(3, cst.WRITE_MULTIPLE_REGISTERS, 0, output_value=[0, 0, 0, 0]))
    logger.info(master_1.execute(1, cst.WRITE_MULTIPLE_COILS, 0, output_value=[0, 0, 0, 0]))

def move(motion, speed):
    logger.info(master_2.execute(3, cst.WRITE_MULTIPLE_REGISTERS, 0, output_value=[speed, speed, speed, speed]))
    if motion == 'forward':
        logger.info(master_1.execute(1, cst.WRITE_MULTIPLE_COILS, 0, output_value=[1, 0, 1, 0]))
    elif motion == 'backward':
        logger.info(master_1.execute(1, cst.WRITE_MULTIPLE_COILS, 0, output_value=[0, 1, 0, 1]))
    elif motion == 'left':
        logger.info(master_1.execute(1, cst.WRITE_MULTIPLE_COILS, 0, output_value=[0, 0, 1, 1]))
    elif motion == 'right':
        logger.info(master_1.execute(1, cst.WRITE_MULTIPLE_COILS, 0, output_value=[1, 1, 0, 0]))
    else:
        stop()

def teleop_callback(data):
    linear_speed = data.linear.x  # Twist消息的线速度
    angular_speed = data.angular.z  # Twist消息的角速度

    # 根据线速度和角速度确定运动方向和速度
    if linear_speed > 0.0:
        move('forward', int(linear_speed * 1000))  # 调整线速度的系数
    elif linear_speed < 0.0:
        move('backward', int(abs(linear_speed) * 1000))
    elif angular_speed > 0.0:
        move('left', int(angular_speed * 1000))
    elif angular_speed < 0.0:
        move('right', int(abs(angular_speed) * 1000))
    else:
        stop()

def main():
    rospy.init_node('motor_control_node')
    rospy.Subscriber('/cmd_vel', Twist, teleop_callback)
    
    try:
        master_1 = modbus_tcp.TcpMaster(host="192.168.255.4")
        master_2 = modbus_tcp.TcpMaster(host="192.168.255.3")
        master_1.set_timeout(5.0)
        master_2.set_timeout(5.0)
        logger.info("connected")

        while not rospy.is_shutdown():
            pass

    except modbus_tk.modbus.ModbusError as e:
        logger.error("%s- Code=%d" % (e, e.get_exception_code()))
    finally:
        stop()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass

3. 編譯 ros 包 `catkin_make`
4. 啟動 ros master `roscore`
5. 運行

rosrun my_motor_control motor_control_node.py

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

6. 用鍵盤控制

teleop_keyboard節點會發布Twist訊息到主題上，你的自訂節點將接收並處理這些訊息，從而控制馬達/cmd_vel的運動。

* `i`鍵：向前運動
* `k`鍵：停止
* `j`鍵：左轉
* `l`鍵：右轉
* `,`鍵：剛性運動
* `.`鍵：旋轉（逆時針）
* `/`鍵：旋轉（順時針）