test

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()
+

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()

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

package.xml

@@ -11,7 +11,7 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>sshkeyboard</exec_depend>
-
+  <exec_depend>launch_ros</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

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