diff --git a/.cspell.json b/.cspell.json index c5f30ee..1919c74 100644 --- a/.cspell.json +++ b/.cspell.json @@ -59,11 +59,13 @@ "pointcloud", "RANSAC", "rclcpp", + "rclpy", "rosdep", "rosdistro", "rosidl", "SACMODEL", "schematypens", + "socketcan", "urdf", "vcstool", "velodyne", diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 06918e6..5c89afc 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,6 +1,7 @@ src/common/wheel_stuck_common_utils/** @Autumn60 @Bey9434 src/control/joy2twist/** @Bey9434 src/description/wheel_stuck_go/** @Autumn60 +src/driver/ddt_motor_driver/** @RyodoTanaka src/description/fourth_robot/** @Autumn60 src/launch/wheel_stuck_launcher/** @Autumn60 src/launch/wheel_stuck_robot_launcher/** @Autumn60 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 36d87a6..0823f04 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -3,7 +3,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.6.0 + rev: v5.0.0 hooks: - id: check-json - id: check-merge-conflict @@ -18,7 +18,7 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.41.0 + rev: v0.42.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] @@ -72,7 +72,7 @@ repos: args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v18.1.8 + rev: v19.1.1 hooks: - id: clang-format types_or: [c++, c, cuda] @@ -85,7 +85,7 @@ repos: exclude: .cu - repo: https://github.com/python-jsonschema/check-jsonschema - rev: 0.29.2 + rev: 0.29.3 hooks: - id: check-metaschema files: ^.+/schema/.*schema\.json$ diff --git a/README.md b/README.md index f2ee05d..39f1ce7 100644 --- a/README.md +++ b/README.md @@ -57,4 +57,22 @@ Fool Stuck Robot ## 2. Run +## 3. Pre-Commit Setup Instructions for Developers + +1. Install pre-commit + + ```bash + pip install pre-commit + ``` + +2. Enable pre-commit hooks + + ```bash + cd /path/to/wheel_stuck_ws + ``` + + ```bash + pre-commit install + ``` + ## LICENSE diff --git a/src/driver/ddt_motor_driver/CMakeLists.txt b/src/driver/ddt_motor_driver/CMakeLists.txt new file mode 100644 index 0000000..22ae322 --- /dev/null +++ b/src/driver/ddt_motor_driver/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.5) +project(ddt_motor_driver) + +find_package(ament_cmake_python REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_python_install_package(${PROJECT_NAME}) + + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/src/driver/ddt_motor_driver/config/.gitkeep b/src/driver/ddt_motor_driver/config/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/driver/ddt_motor_driver/ddt_motor_driver/__init__.py b/src/driver/ddt_motor_driver/ddt_motor_driver/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/driver/ddt_motor_driver/ddt_motor_driver/ddt_motor_driver.py b/src/driver/ddt_motor_driver/ddt_motor_driver/ddt_motor_driver.py new file mode 100644 index 0000000..8355958 --- /dev/null +++ b/src/driver/ddt_motor_driver/ddt_motor_driver/ddt_motor_driver.py @@ -0,0 +1,154 @@ +import math +import time + +import can + + +class DDTMotorDriver: + def __init__(self, max_speed_mps, wheel_diameter, max_acceleration_mps2): + """ + max_speed_mps: モーターの最大速度 [m/s] + wheel_diameter: ホイールの直径 [m]（モーターが車輪に接続されている場合） + max_acceleration_mps2: 最大加速度 [m/s^2] + """ + self.max_speed_mps = max_speed_mps + self.wheel_circumference = math.pi * wheel_diameter # 円周を計算 + self.max_acceleration_mps2 = max_acceleration_mps2 # 最大加速度 + self.current_speeds = [0.0] * 8 # 現在の各モーターの速度[m/s] + self.bus = can.interface.Bus(channel="can0", interface="socketcan") # CANインターフェース + + # 初期化: モーターを初期化し、速度制御モードに切り替える + self.initialize_motor() + + def initialize_motor(self): + self.set_mode_enable() + self.set_mode_velocity_control() + + def mps_to_rpm(self, speed_mps): + """ + m/s から RPM (回転数) に変換する + """ + rpm = (speed_mps / self.wheel_circumference) * 60 + return int(rpm) + + def set_mode_enable(self): + # ID 1 to 8 をenableに + init_command = [0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A] + msg = can.Message(arbitration_id=0x105, data=init_command, is_extended_id=False) + try: + self.bus.send(msg) + time.sleep(1) + print("モーターを初期化しました。") + except can.CanError as e: + print(f"モーター初期化に失敗しました: {e}") + + def set_mode_velocity_control(self): + """ + モーターを速度制御モードに設定する + """ + velocity_control_command = [0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02] + msg = can.Message(arbitration_id=0x105, data=velocity_control_command, is_extended_id=False) + try: + self.bus.send(msg) + print("モーターを速度制御モードに設定しました。") + except can.CanError as e: + print(f"速度制御モード設定に失敗しました: {e}") + + def set_mode_disable(self): + """ + モーターをDisableにする + """ + torque_control_command = [0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09] + msg = can.Message(arbitration_id=0x105, data=torque_control_command, is_extended_id=False) + try: + self.bus.send(msg) + print("モーターをトルク制御モードに設定しました。") + except can.CanError as e: + print(f"トルク制御モード設定に失敗しました: {e}") + + def set_speed(self, target_speeds_mps): + """ + 速度指令を与える際に加速度制限を加える + target_speeds_mps: 各モーターの目標速度 [m/s] の配列 + """ + if len(target_speeds_mps) != 8: + raise ValueError("速度指令はID 1〜8までの設定を配列で与える必要があります。") + + rpm_list = [0.0] * 8 + + for i, target_speed_mps in enumerate(target_speeds_mps): + """ + 指定された速度[m/s]をモーターに送信する前に、加速度制限を考慮して速度を調整 + target_speed_mps: 目標速度 [m/s] + """ + if abs(target_speed_mps) > self.max_speed_mps: + raise ValueError("指定された速度が最大速度を超えています！") + + # 加速度制限を考慮して新しい速度を決定 + speed_diff = target_speed_mps - self.current_speeds[i] + max_speed_change = ( + self.max_acceleration_mps2 * 0.1 + ) # 加速度制限に基づいた最大速度変化量（0.1秒ごとに制御） + + if abs(speed_diff) > max_speed_change: + # 加速度制限を超えないように調整 + speed_diff = max_speed_change if speed_diff > 0 else -max_speed_change + + # 更新された速度 + self.current_speeds[i] += speed_diff + rpm_list[i] = self.mps_to_rpm(self.current_speeds[i]) + + # CAN メッセージを構築して送信 + speed_command0 = [ + (rpm_list[0] >> 8) & 0xFF, + rpm_list[0] & 0xFF, + (rpm_list[1] >> 8) & 0xFF, + rpm_list[1] & 0xFF, + (rpm_list[2] >> 8) & 0xFF, + rpm_list[2] & 0xFF, + (rpm_list[3] >> 8) & 0xFF, + rpm_list[3] & 0xFF, + ] + speed_command1 = [ + (rpm_list[4] >> 8) & 0xFF, + rpm_list[4] & 0xFF, + (rpm_list[5] >> 8) & 0xFF, + rpm_list[5] & 0xFF, + (rpm_list[6] >> 8) & 0xFF, + rpm_list[6] & 0xFF, + (rpm_list[7] >> 8) & 0xFF, + rpm_list[7] & 0xFF, + ] + + msg0 = can.Message(arbitration_id=0x32, data=speed_command0, is_extended_id=False) + msg1 = can.Message(arbitration_id=0x33, data=speed_command1, is_extended_id=False) + + try: + self.bus.send(msg0) + self.bus.send(msg1) + except can.CanError as e: + print(f"速度設定に失敗しました: {e}") + + def stop_motor(self): + """ + モーターを停止する + """ + self.set_speed([0, 0, 0, 0, 0, 0, 0, 0]) # 速度を0に設定 + self.set_mode_disable() # モーターを無効にする + print("モーターを停止しました。") + + def shutdown(self): + """ + CANバスを閉じる + """ + self.bus.shutdown() + print("CANバスをシャットダウンしました。") + + +# 使用例: +# if __name__ == "__main__": +# motor_controller = DDTMotorDriver(max_speed_mps=1.6, wheel_diameter=0.185, max_acceleration_mps2=0.5) +# motor_controller.set_speed([1.1, -1.1, 0, 0, 0, 0, 0, 0]) +# time.sleep(3) +# motor_controller.stop_motor() # モーターを停止 +# motor_controller.shutdown() # CANバスを閉じる diff --git a/src/driver/ddt_motor_driver/ddt_motor_driver/diff_drive_controller.py b/src/driver/ddt_motor_driver/ddt_motor_driver/diff_drive_controller.py new file mode 100644 index 0000000..bbda423 --- /dev/null +++ b/src/driver/ddt_motor_driver/ddt_motor_driver/diff_drive_controller.py @@ -0,0 +1,56 @@ +import math + + +class DiffDrive: + def __init__(self, wheel_radius, wheel_base): + """ + コンストラクタ + wheel_radius: 車輪の半径（m） + wheel_base: 左右の車輪間の距離（m） + """ + self.wheel_radius = wheel_radius # 車輪の半径 + self.wheel_base = wheel_base # 車輪間の距離 + + def calculate_velocity(self, left_wheel_speed, right_wheel_speed): + """ + 左右の車輪速度（m/s）から直線速度と角速度を計算する + left_wheel_speed: 左車輪の速度（m/s） + right_wheel_speed: 右車輪の速度（m/s） + """ + # 直線速度 (linear velocity) + linear_velocity = (left_wheel_speed + right_wheel_speed) / 2 + + # 角速度 (angular velocity) + angular_velocity = (right_wheel_speed - left_wheel_speed) / self.wheel_base + + return linear_velocity, angular_velocity + + def calculate_wheel_speeds(self, linear_velocity, angular_velocity): + """ + 直線速度と角速度から左右の車輪の速度を計算する + linear_velocity: 直線速度（m/s） + angular_velocity: 角速度（rad/s） + """ + # 左右の車輪の速度 (m/s) を計算 + left_wheel_speed = linear_velocity - (angular_velocity * self.wheel_base) / 2 + right_wheel_speed = linear_velocity + (angular_velocity * self.wheel_base) / 2 + + return left_wheel_speed, right_wheel_speed + + +# 使用例 +if __name__ == "__main__": + # 車輪の半径が0.1m、車輪間の距離が0.5mの場合 + robot = DiffDrive(wheel_radius=0.1, wheel_base=0.5) + + # 左右の車輪の直線速度 (m/s) + left_speed = 1.0 # 左車輪 + right_speed = 1.5 # 右車輪 + + # 直線速度と角速度を計算 + linear_vel, angular_vel = robot.calculate_velocity(left_speed, right_speed) + print(f"直線速度: {linear_vel} m/s, 角速度: {angular_vel} rad/s") + + # 直線速度1.0 m/s, 角速度0.5 rad/sに対して左右の車輪速度を計算 + left_wheel, right_wheel = robot.calculate_wheel_speeds(linear_vel, angular_vel) + print(f"左車輪の速度: {left_wheel} m/s, 右車輪の速度: {right_wheel} m/s") diff --git a/src/driver/ddt_motor_driver/ddt_motor_driver/wheel_stuck_driver.py b/src/driver/ddt_motor_driver/ddt_motor_driver/wheel_stuck_driver.py new file mode 100644 index 0000000..c5a7d8e --- /dev/null +++ b/src/driver/ddt_motor_driver/ddt_motor_driver/wheel_stuck_driver.py @@ -0,0 +1,236 @@ +import math +import time + +import can +import rclpy +from geometry_msgs.msg import TwistStamped +from rclpy.node import Node + + +class DDTMotorDriver: + def __init__(self, max_speed_mps, wheel_diameter, max_acceleration_mps2): + """ + max_speed_mps: モーターの最大速度 [m/s] + wheel_diameter: ホイールの直径 [m]（モーターが車輪に接続されている場合） + max_acceleration_mps2: 最大加速度 [m/s^2] + """ + self.max_speed_mps = max_speed_mps + self.wheel_circumference = math.pi * wheel_diameter # 円周を計算 + self.max_acceleration_mps2 = max_acceleration_mps2 # 最大加速度 + self.current_speeds = [0.0] * 8 # 現在の各モーターの速度[m/s] + self.bus = can.interface.Bus(channel="can0", interface="socketcan") # CANインターフェース + + # 初期化: モーターを初期化し、速度制御モードに切り替える + self.initialize_motor() + + def initialize_motor(self): + self.set_mode_enable() + self.set_mode_velocity_control() + + def mps_to_rpm(self, speed_mps): + """ + m/s から RPM (回転数) に変換する + """ + rpm = (speed_mps / self.wheel_circumference) * 60 + return int(rpm) + + def set_mode_enable(self): + # ID 1 to 8 をenableに + init_command = [0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A] + msg = can.Message(arbitration_id=0x105, data=init_command, is_extended_id=False) + try: + self.bus.send(msg) + time.sleep(1) + print("モーターを初期化しました。") + except can.CanError as e: + print(f"モーター初期化に失敗しました: {e}") + + def set_mode_velocity_control(self): + """ + モーターを速度制御モードに設定する + """ + velocity_control_command = [0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02] + msg = can.Message(arbitration_id=0x105, data=velocity_control_command, is_extended_id=False) + try: + self.bus.send(msg) + print("モーターを速度制御モードに設定しました。") + except can.CanError as e: + print(f"速度制御モード設定に失敗しました: {e}") + + def set_mode_disable(self): + """ + モーターをDisableにする + """ + torque_control_command = [0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09] + msg = can.Message(arbitration_id=0x105, data=torque_control_command, is_extended_id=False) + try: + self.bus.send(msg) + print("モーターをトルク制御モードに設定しました。") + except can.CanError as e: + print(f"トルク制御モード設定に失敗しました: {e}") + + def set_speed(self, target_speeds_mps): + """ + 速度指令を与える際に加速度制限を加える + target_speeds_mps: 各モーターの目標速度 [m/s] の配列 + """ + if len(target_speeds_mps) != 8: + raise ValueError("速度指令はID 1〜8までの設定を配列で与える必要があります。") + + rpm_list = [0.0] * 8 + + for i, target_speed_mps in enumerate(target_speeds_mps): + """ + 指定された速度[m/s]をモーターに送信する前に、加速度制限を考慮して速度を調整 + target_speed_mps: 目標速度 [m/s] + """ + if abs(target_speed_mps) > self.max_speed_mps: + raise ValueError("指定された速度が最大速度を超えています！") + + # 加速度制限を考慮して新しい速度を決定 + speed_diff = target_speed_mps - self.current_speeds[i] + max_speed_change = ( + self.max_acceleration_mps2 * 0.1 + ) # 加速度制限に基づいた最大速度変化量（0.1秒ごとに制御） + + if abs(speed_diff) > max_speed_change: + # 加速度制限を超えないように調整 + speed_diff = max_speed_change if speed_diff > 0 else -max_speed_change + + # 更新された速度 + self.current_speeds[i] += speed_diff + rpm_list[i] = self.mps_to_rpm(self.current_speeds[i]) + + # CAN メッセージを構築して送信 + speed_command0 = [ + (rpm_list[0] >> 8) & 0xFF, + rpm_list[0] & 0xFF, + (rpm_list[1] >> 8) & 0xFF, + rpm_list[1] & 0xFF, + (rpm_list[2] >> 8) & 0xFF, + rpm_list[2] & 0xFF, + (rpm_list[3] >> 8) & 0xFF, + rpm_list[3] & 0xFF, + ] + speed_command1 = [ + (rpm_list[4] >> 8) & 0xFF, + rpm_list[4] & 0xFF, + (rpm_list[5] >> 8) & 0xFF, + rpm_list[5] & 0xFF, + (rpm_list[6] >> 8) & 0xFF, + rpm_list[6] & 0xFF, + (rpm_list[7] >> 8) & 0xFF, + rpm_list[7] & 0xFF, + ] + + msg0 = can.Message(arbitration_id=0x32, data=speed_command0, is_extended_id=False) + msg1 = can.Message(arbitration_id=0x33, data=speed_command1, is_extended_id=False) + + try: + self.bus.send(msg0) + self.bus.send(msg1) + except can.CanError as e: + print(f"速度設定に失敗しました: {e}") + + def stop_motor(self): + """ + モーターを停止する + """ + self.set_speed([0, 0, 0, 0, 0, 0, 0, 0]) # 速度を0に設定 + self.set_mode_disable() # モーターを無効にする + print("モーターを停止しました。") + + def shutdown(self): + """ + CANバスを閉じる + """ + self.bus.shutdown() + print("CANバスをシャットダウンしました。") + + +class DiffDrive: + def __init__(self, wheel_radius, wheel_base): + """ + コンストラクタ + wheel_radius: 車輪の半径（m） + wheel_base: 左右の車輪間の距離（m） + """ + self.wheel_radius = wheel_radius # 車輪の半径 + self.wheel_base = wheel_base # 車輪間の距離 + + def calculate_velocity(self, left_wheel_speed, right_wheel_speed): + """ + 左右の車輪速度（m/s）から直線速度と角速度を計算する + left_wheel_speed: 左車輪の速度（m/s） + right_wheel_speed: 右車輪の速度（m/s） + """ + # 直線速度 (linear velocity) + linear_velocity = (left_wheel_speed + right_wheel_speed) / 2 + + # 角速度 (angular velocity) + angular_velocity = (right_wheel_speed - left_wheel_speed) / self.wheel_base + + return linear_velocity, angular_velocity + + def calculate_wheel_speeds(self, linear_velocity, angular_velocity): + """ + 直線速度と角速度から左右の車輪の速度を計算する + linear_velocity: 直線速度（m/s） + angular_velocity: 角速度（rad/s） + """ + # 左右の車輪の速度 (m/s) を計算 + left_wheel_speed = linear_velocity - (angular_velocity * self.wheel_base) / 2 + right_wheel_speed = linear_velocity + (angular_velocity * self.wheel_base) / 2 + + return left_wheel_speed, right_wheel_speed + + +class TwistSubscriber(Node): + + def __init__(self): + super().__init__("twist_subscriber") + + self.ddt_driver = DDTMotorDriver( + max_speed_mps=1.6, wheel_diameter=0.185, max_acceleration_mps2=0.2 + ) + self.diff_driver = DiffDrive(wheel_radius=0.0925, wheel_base=0.265) + + # Twistメッセージを購読するサブスクライバを作成 + self.subscription = self.create_subscription( + TwistStamped, + "turtle1/cmd_vel", # 購読するトピック名（デフォルトは cmd_vel） + self.listener_callback, + 10, + ) # キューサイズ + + self.subscription # 変数を保持しておく + + def __del__(self): + self.ddt_driver.stop_motor() + self.ddt_driver.shutdown() + + def listener_callback(self, msg): + + left, right = self.diff_driver.calculate_wheel_speeds( + msg.twist.linear.x, msg.twist.angular.z + ) + self.ddt_driver.set_speed([left, -right, 0, 0, 0, 0, 0, 0]) + + +def main(args=None): + # rclpyの初期化 + rclpy.init(args=args) + + # ノードを作成し、サブスクライバを開始 + twist_subscriber = TwistSubscriber() + + # ノードをスピンして実行（Ctrl+Cで停止） + rclpy.spin(twist_subscriber) + + # ノードをシャットダウン + twist_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/driver/ddt_motor_driver/launch/.gitkeep b/src/driver/ddt_motor_driver/launch/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/driver/ddt_motor_driver/package.xml b/src/driver/ddt_motor_driver/package.xml new file mode 100644 index 0000000..880b8e5 --- /dev/null +++ b/src/driver/ddt_motor_driver/package.xml @@ -0,0 +1,19 @@ + + + + ddt_motor_driver + 0.1.0 + The ddt_motor_driver package + Ryodo Tanaka + + Apache 2 + + ament_cmake_auto + ament_cmake_python + + ament_lint_auto + + + ament_cmake + +