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',
],
},
)