diff --git a/keyboard_teleop/keyboard_receive.py b/keyboard_teleop/keyboard_receive.py new file mode 100644 index 0000000..1c714b0 --- /dev/null +++ b/keyboard_teleop/keyboard_receive.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: 2025 Kenta Hirachi +# SPDX-License=Identifier: Apache 2.0 + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist + +class keyboard_receive(Node): + def __init__(self): + super().__init__('cmd_vel_subscriber') + self.subscription = self.create_subscription(Twist,'cmd_vel',self.listener_callback,10) + self.subscription + + def listener_callback(self, msg): + + self.get_logger().info(f"Received Twist message: linear.x = {msg.linear.x}, linear.y = {msg.linear.y}, angular.z = {msg.angular.z}") + +def main(): + rclpy.init() + node = keyboard_receive() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() + diff --git a/keyboard_teleop/keyboard_teleop.py b/keyboard_teleop/keyboard_teleop.py old mode 100644 new mode 100755 index 9446fda..0578b98 --- a/keyboard_teleop/keyboard_teleop.py +++ b/keyboard_teleop/keyboard_teleop.py @@ -1,106 +1,101 @@ #!/usr/bin/env python3 # SPDX-FileCopyrightText: 2025 Kenta Hirachi -# SPDX-License=Identifier: Apache 2.0 +# SPDX-License-Identifier: Apache 2.0 import rclpy from rclpy.node import Node -from geometry_msgs.msg import Twist # Twist メッセージ型をインポート -from sshkeyboard import listen_keyboard +from geometry_msgs.msg import Twist +import sys +import termios +import tty +import select -class KeyboardTeleop(Node): - def __init__(self): # コンストラクタ - super().__init__('keyboard_teleop_node') + +instructions = """ +--------------------------- +Moving around: + + q w e + a d + z s c + +j: Increase speed +k: Decrease speed +l: Reset speed to 0.5 +--------------------------- +""" + + +key_mapping = { + 'w': (1, 0), + 's': (-1, 0), + 'a': (0, -1), + 'd': (0, 1), + 'q': (1, -1), + 'e': (1, 1), + 'z': (-1, -1), + 'c': (-1, 1), +} + +class TeleopTwistKeyboard(Node): + def __init__(self): + super().__init__('teleop_twist_keyboard') self.publisher = self.create_publisher(Twist, 'cmd_vel', 10) - self.twist = Twist() self.speed = 0.5 - self.pressed_keys = set() # 押されているキーを管理するセット - - self.display_instructions() - - def display_instructions(self): - msg = """ - --------------------------- - Moving around: - - q e - w - - a d - - s - z c - - j: Increase x, y speed - k: Decrease x, y speed - l: Reset x, y speed to 0.5 - --------------------------- - """ - self.get_logger().info(msg) - - def process_keys(self): - - if 'q' in self.pressed_keys: # 左上 - self.twist.linear.x = self.speed - self.twist.linear.y = -self.speed - elif 'e' in self.pressed_keys: # 右上 - self.twist.linear.x = self.speed - self.twist.linear.y = self.speed - elif 'z' in self.pressed_keys: # 左下 - self.twist.linear.x = -self.speed - self.twist.linear.y = -self.speed - elif 'c' in self.pressed_keys: # 右下 - self.twist.linear.x = -self.speed - self.twist.linear.y = self.speed - elif 'w' in self.pressed_keys: # 前進 - self.twist.linear.x = self.speed - self.twist.linear.y = 0.0 - elif 's' in self.pressed_keys: # 後退 - self.twist.linear.x = -self.speed - self.twist.linear.y = 0.0 - elif 'a' in self.pressed_keys: # 左移動 - self.twist.linear.x = 0.0 - self.twist.linear.y = -self.speed - elif 'd' in self.pressed_keys: # 右移動 - self.twist.linear.x = 0.0 - self.twist.linear.y = self.speed - else: # 停止 - self.twist.linear.x = 0.0 - self.twist.linear.y = 0.0 + self.twist = Twist() + print(instructions) + + def get_key(self): - self.publisher.publish(self.twist) + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(fd) + rlist, _, _ = select.select([sys.stdin], [], [], 0.1) + if rlist: + return sys.stdin.read(1) + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return None - def handle_key_press(self, key): - - self.pressed_keys.add(key) - if key == 'j': # 速度アップ - self.speed += 0.1 - self.get_logger().info(f"Speed increased to {self.speed}") - elif key == 'k': # 速度ダウン - self.speed = max(0.1, self.speed - 0.1) - self.get_logger().info(f"Speed decreased to {self.speed}") - elif key == 'l': # 速度リセット - self.speed = 0.5 - self.get_logger().info(f"Speed decreased to {self.speed}") - self.process_keys() + def run(self): + try: + while rclpy.ok(): + key = self.get_key() + if key in key_mapping: + self.twist.linear.x = self.speed * key_mapping[key][0] + self.twist.linear.y = self.speed * key_mapping[key][1] + elif key == 'j': + self.speed += 0.1 + print(f"Speed increased to {self.speed}") + elif key == 'k': + self.speed = max(0.1, self.speed - 0.1) + print(f"Speed decreased to {self.speed}") + elif key == 'l': # + self.speed = 0.5 + print("Speed reset to 0.5") + elif key == '\x03': + break + else: + self.twist.linear.x = 0.0 + self.twist.linear.y = 0.0 - def handle_key_release(self, key): + self.publisher.publish(self.twist) - self.pressed_keys.discard(key) - self.process_keys() + except Exception as e: + self.get_logger().error(f"Error: {e}") + finally: + self.twist.linear.x = 0.0 + self.twist.linear.y = 0.0 + self.publisher.publish(self.twist) + print("\nExiting teleop_twist_keyboard...") def main(): rclpy.init() - node = KeyboardTeleop() - try: - listen_keyboard( - on_press=node.handle_key_press, - on_release=node.handle_key_release, - ) - except Exception as e: - node.get_logger().error(f"Error: {e}") - finally: - rclpy.try_shutdown() + node = TeleopTwistKeyboard() + node.run() + rclpy.shutdown() if __name__ == '__main__': main() diff --git a/launch/keyboard_teleop.launch.py b/launch/keyboard_teleop.launch.py new file mode 100644 index 0000000..ad3c8a9 --- /dev/null +++ b/launch/keyboard_teleop.launch.py @@ -0,0 +1,23 @@ +# SPDX-FileCopyrightText: 2025 Kenta Hirachi +# SPDX-License=Identifier: Apache 2.0 + +import launch +import launch.actions +import launch.substitutions +import launch_ros.actions + +def generate_launch_description(): + return launch.LaunchDescription([ + + launch_ros.actions.Node( + package='keyboard_teleop', + executable='keyboard_teleop' + ), + + launch_ros.actions.Node( + package='keyboard_teleop', + executable='keyboard_receive', + output='screen', + ), + ]) + diff --git a/package.xml b/package.xml index 49505da..77c9ef6 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,7 @@ rclpy geometry_msgs sshkeyboard - + launch_ros ament_copyright ament_flake8 diff --git a/setup.py b/setup.py index 8fdfd84..9b6a7d9 100644 --- a/setup.py +++ b/setup.py @@ -16,6 +16,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*.launch.py')) ], install_requires=['setuptools'], zip_safe=True, @@ -27,6 +28,7 @@ entry_points={ 'console_scripts': [ 'keyboard_teleop = keyboard_teleop.keyboard_teleop:main', + 'keyboard_receive = keyboard_teleop.keyboard_receive:main', ], }, )