w : زيادة السرعة الأمامية (+0.1 م/ث لكل ضغطة)
x : زيادة السرعة الخلفية (-0.1 م/ث لكل ضغطة)
a : زيادة معدل الدوران لليسار (+0.2 راد/ث لكل ضغطة)
d : زيادة معدل الدوران لليمين (-0.2 راد/ث لكل ضغطة)
s : توقف (ضبط جميع السرعات على صفر)
SPACE : إيقاف طارئ (نفس تأثير 's')
q : الخروج من البرنامج

cd ~/Desktop/ROS-2-Practical-Course-Roadmap-2025/ros_ws/warehouse_simulation/warehouse_simulation
nano teleop_key.py

#!/usr/bin/env python3
"""
Teleop Key Node for Warehouse Robot
Control the robot using keyboard keys:
    w/x : increase/decrease linear velocity
    a/d : increase/decrease angular velocity
    s   : stop
    SPACE : emergency stop
    q   : quit
"""
 
import sys
import tty
import termios
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
 
 
class TeleopKeyNode(Node):
    def __init__(self):
        super().__init__('teleop_key')
 
        # Publisher for cmd_vel
        self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)
 
        # Velocity settings
        self.linear_velocity = 0.0
        self.angular_velocity = 0.0
        self.linear_step = 0.1  # m/s
        self.angular_step = 0.2  # rad/s
        self.max_linear = 1.0
        self.max_angular = 2.0
 
        self.get_logger().info('Teleop Key Node Started!')
        self.get_logger().info('Use w/x for linear, a/d for angular, s to stop, SPACE for emergency stop, q to quit')
 
    def get_key(self):
        """Get a single keypress from the terminal"""
        fd = sys.stdin.fileno()
        old_settings = termios.tcgetattr(fd)
        try:
            tty.setraw(fd)
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch
 
    def update_velocity(self, linear_change, angular_change):
        """Update velocity with limits"""
        self.linear_velocity += linear_change
        self.angular_velocity += angular_change
 
        # Apply limits
        self.linear_velocity = max(-self.max_linear, min(self.max_linear, self.linear_velocity))
        self.angular_velocity = max(-self.max_angular, min(self.max_angular, self.angular_velocity))
 
    def publish_velocity(self):
        """Publish the current velocity"""
        msg = Twist()
        msg.linear.x = self.linear_velocity
        msg.angular.z = self.angular_velocity
        self.publisher.publish(msg)
 
        self.get_logger().info(f'Linear: {self.linear_velocity:.2f} m/s, Angular: {self.angular_velocity:.2f} rad/s')
 
    def run(self):
        """Main control loop"""
        print("\n" + "="*60)
        print("Warehouse Robot Teleop Control")
        print("="*60)
        print("Controls:")
        print("  w : Increase forward speed")
        print("  x : Increase backward speed")
        print("  a : Turn left (increase)")
        print("  d : Turn right (increase)")
        print("  s : Stop all movement")
        print("  SPACE : Emergency stop")
        print("  q : Quit")
        print("="*60 + "\n")
 
        try:
            while rclpy.ok():
                key = self.get_key()
 
                if key == 'w':
                    self.update_velocity(self.linear_step, 0)
                    self.publish_velocity()
 
                elif key == 'x':
                    self.update_velocity(-self.linear_step, 0)
                    self.publish_velocity()
 
                elif key == 'a':
                    self.update_velocity(0, self.angular_step)
                    self.publish_velocity()
 
                elif key == 'd':
                    self.update_velocity(0, -self.angular_step)
                    self.publish_velocity()
 
                elif key == 's' or key == ' ':
                    self.linear_velocity = 0.0
                    self.angular_velocity = 0.0
                    self.publish_velocity()
                    if key == ' ':
                        self.get_logger().warn('EMERGENCY STOP!')
 
                elif key == 'q':
                    self.get_logger().info('Quitting...')
                    break
 
                elif key == '\x03':  # Ctrl+C
                    break
 
        except Exception as e:
            self.get_logger().error(f'Error: {e}')
 
        finally:
            # Stop the robot before exiting
            self.linear_velocity = 0.0
            self.angular_velocity = 0.0
            self.publish_velocity()
            self.get_logger().info('Teleop Key Node Stopped')
 
 
def main(args=None):
    rclpy.init(args=args)
 
    teleop_node = TeleopKeyNode()
 
    try:
        teleop_node.run()
    except KeyboardInterrupt:
        pass
    finally:
        teleop_node.destroy_node()
        rclpy.shutdown()
 
 
if __name__ == '__main__':
    main()

chmod +x ~/Desktop/ROS-2-Practical-Course-Roadmap-2025/ros_ws/warehouse_simulation/warehouse_simulation/teleop_key.py

cd ~/Desktop/ROS-2-Practical-Course-Roadmap-2025/ros_ws/warehouse_simulation
nano setup.py

entry_points={
    'console_scripts': [
        'teleop_key = warehouse_simulation.teleop_key:main',
    ],
},

cd ~/Desktop/ROS-2-Practical-Course-Roadmap-2025/ros_ws
colcon build --packages-select warehouse_simulation
source install/setup.bash

Starting >>> warehouse_simulation
Finished <<< warehouse_simulation [X.XXs]
 
Summary: 1 package finished [X.XXs]

cd ~/Desktop/ROS-2-Practical-Course-Roadmap-2025/ros_ws
source install/setup.bash
ros2 launch warehouse_simulation warehouse_simulation.launch.py

cd ~/Desktop/ROS-2-Practical-Course-Roadmap-2025/ros_ws
source install/setup.bash
ros2 run warehouse_simulation teleop_key

============================================================
Warehouse Robot Teleop Control
============================================================
Controls:
  w : Increase forward speed
  x : Increase backward speed
  a : Turn left (increase)
  d : Turn right (increase)
  s : Stop all movement
  SPACE : Emergency stop
  q : Quit
============================================================
 
[INFO] [teleop_key]: Teleop Key Node Started!
[INFO] [teleop_key]: Use w/x for linear, a/d for angular, s to stop, SPACE for emergency stop, q to quit

class TeleopKeyNode(Node):
    def __init__(self):
        super().__init__('teleop_key')
        self.publisher = self.create_publisher(Twist, '/cmd_vel', 10)

def get_key(self):
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)
        ch = sys.stdin.read(1)
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return ch

def update_velocity(self, linear_change, angular_change):
    self.linear_velocity += linear_change
    self.angular_velocity += angular_change
 
    # Apply limits
    self.linear_velocity = max(-self.max_linear, min(self.max_linear, self.linear_velocity))

def publish_velocity(self):
    msg = Twist()
    msg.linear.x = self.linear_velocity
    msg.angular.z = self.angular_velocity
    self.publisher.publish(msg)

source ~/Desktop/ROS-2-Practical-Course-Roadmap-2025/ros_ws/install/setup.bash
ros2 topic echo /cmd_vel

linear:
  x: 0.3
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.2
---

ros2 node list
# Should show: /teleop_key
 
ros2 node info /teleop_key
# Shows publishers, subscribers, etc.

ros2 run rqt_graph rqt_graph

/teleop_key --> /cmd_vel --> /parameter_bridge --> Gazebo

# In the run() method, add:
elif key == '1':
    self.linear_step = 0.05
    self.get_logger().info('Speed: SLOW')
elif key == '2':
    self.linear_step = 0.1
    self.get_logger().info('Speed: NORMAL')
elif key == '3':
    self.linear_step = 0.2
    self.get_logger().info('Speed: FAST')

# Add subscriber in __init__:
self.odom_sub = self.create_subscription(
    Odometry, '/odom', self.odom_callback, 10
)
 
# Add callback:
def odom_callback(self, msg):
    self.position = msg.pose.pose.position

# Add subscriber for laser scan:
self.scan_sub = self.create_subscription(
    LaserScan, '/scan', self.scan_callback, 10
)
 
def scan_callback(self, msg):
    min_dist = min(msg.ranges)
    if min_dist < 0.5:  # 50cm
        self.get_logger().warn(f'Obstacle detected at {min_dist:.2f}m!')

sudo apt install ros-jazzy-teleop-twist-keyboard
ros2 run teleop_twist_keyboard teleop_twist_keyboard

sudo apt install ros-jazzy-joy ros-jazzy-teleop-twist-joy
 
# Terminal 1
ros2 run joy joy_node
 
# Terminal 2
ros2 run teleop_twist_joy teleop_node

# Is simulation running?
ros2 topic list | grep cmd_vel
 
# Is teleop publishing?
ros2 topic echo /cmd_vel
 
# Are there multiple publishers?
ros2 topic info /cmd_vel

ros2 topic echo /cmd_vel

warehouse_simulation/
├── warehouse_simulation/
│   ├── __init__.py
│   └── teleop_key.py          ← Created in this lab
├── config/
│   └── bridge.yaml
├── launch/
│   └── warehouse_simulation.launch.py
├── package.xml
└── setup.py                   ← Updated in this lab

# Build
colcon build --packages-select warehouse_simulation
 
# Launch simulation (Terminal 1)
ros2 launch warehouse_simulation warehouse_simulation.launch.py
 
# Run teleop (Terminal 2)
ros2 run warehouse_simulation teleop_key
 
# Monitor velocity (Terminal 3)
ros2 topic echo /cmd_vel
 
# Check nodes
ros2 node list
ros2 node info /teleop_key
 
# Visualize
ros2 run rqt_graph rqt_graph

w     - Forward speed +0.1 m/s
x     - Backward speed -0.1 m/s
a     - Turn left +0.2 rad/s
d     - Turn right -0.2 rad/s
s     - Stop
SPACE - Emergency stop
q     - Quit