Teleop robot

This project demonstrates how to use Genesys to spawn robots into a Gazebo simulation and control them via a custom teleoperation node.

🛠 Requirements

ROS 2 Humble or newer
Gazebo Classic with gazebo_ros_pkgs
Python ≥ 3.8
Genesys (install from PyPI or local dev build)

pip install genesys-framework-cli

Add genesys to your bashrc

echo 'export PATH="$(python3 -m site --user-base)/bin:$PATH"' >> ~/.bashrc

Create a new genesys project

genesys new robot

cd robot

Build your workspace

genesys build

Visit https://app.gazebosim.org/ to get the urdf model we will be using for this tutorial. The pioneer 2DX

Download the zip file and extract the content

Copy the content into your sim/models directory/folder

Create an empty world file in the sim/worlds directory called empty.world

<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="empty_world">
    <light name="sun" type="directional">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 -0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 -0.5 -1</direction>
    </light>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type="adiabatic"/>
    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>0</latitude_deg>
      <longitude_deg>0</longitude_deg>
      <elevation>0</elevation>
    </spherical_coordinates>
    <state world_name="empty_world">
      <sim_time>0 0</sim_time>
      <real_time>0 0</real_time>
      <wall_time>0 0</wall_time>
      <iterations>0</iterations>
      <model name="ground_plane">
        <pose>0 0 0 0 -0 0</pose>
        <scale>1 1 1</scale>
        <link name="link">
          <pose>0 0 0 0 -0 0</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 0 0 -0 0</acceleration>
          <wrench>0 0 0 0 -0 0</wrench>
        </link>
      </model>
    </state>
    <gui fullscreen="0">
      <camera name="user_camera">
        <pose>5 -5 2 0 0.275 2.356</pose>
      </camera>
    </gui>
    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>100</mu>
                <mu2>50</mu2>
              </ode>
            </friction>
            <bounce/>
            <contact>
              <ode/>
            </contact>
          </surface>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0 0 0 1</ambient>
            <diffuse>0.7 0.7 0.7 1</diffuse>
            <specular>0.01 0.01 0.01 1</specular>
            <emissive>0 0 0 1</emissive>
          </material>
        </visual>
        <pose>0 0 0 0 -0 0</pose>
        <self_collide>false</self_collide>
        <enable_wind>false</enable_wind>
        <kinematic>false</kinematic>
      </link>
    </model>
  </world>
</sdf>

After the world and model have been placed in their respective directories, create a package with a node

genesys make pkg teleop_pkg --with-node

A node called teleop_pkg_node will be created and registered. A launch file for the pkg will be created as well but won't be needed for now.

In the node, replace the content with this

from genesys.decorators import node, publisher, timer
from genesys.helpers import spin_node
from geometry_msgs.msg import Twist
import sys, termios, tty, select, os

def has_tty():
    return os.isatty(sys.stdin.fileno())

# --- non-blocking key reader (no echo) ---
def get_key_nonblocking():
    if not has_tty():
        return None   # no keyboard available
    
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        # raw mode, no echo
        tty.setraw(fd)
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        rlist, _, _ = select.select([sys.stdin], [], [], 0)
        if rlist:
            return sys.stdin.read(1)
        return None
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)


@node("teleop")
class Teleop:
    """
    Teleop node that publishes velocity commands to /cmd_vel
    using WASD keys. Space stops, 'q' quits.
    """

    def __init__(self):
        self.twist = Twist()
        self.logger.info("Teleop started. Use WASD to move, space to stop, 'q' to quit.")

    @timer(period_sec=0.1)  # 10 Hz loop
    @publisher(topic="cmd_vel", msg_type=Twist)
    def publish_cmd(self):
        key = get_key_nonblocking()

        # reset to zero each cycle unless overridden
        self.twist.linear.x = 0.0
        self.twist.angular.z = 0.0

        if key == 'w':
            self.twist.linear.x = 1.5
        elif key == 's':
            self.twist.linear.x = -1.5
        elif key == 'a':
            self.twist.angular.z = 1.5
        elif key == 'd':
            self.twist.angular.z = -1.5
        elif key == 'q':
            self.logger.info("Quitting teleop…")
            raise SystemExit
        # space already handled by reset-to-zero above

        return self.twist


def main(args=None):
    spin_node(Teleop, args)


if __name__ == "__main__":
    main()

This script is a teleoperation node. Let’s break it down step by step:

1. Imports

from genesys.decorators import node, publisher, timer
from genesys.helpers import spin_node
from geometry_msgs.msg import Twist
import sys, termios, tty, select, os

Genesys decorators/helpers:
- @node declares a ROS 2 node.
- @publisher makes the function publish a message on a topic.
- @timer calls a function periodically (like ros2 Timer).
- spin_node runs the node (similar to rclpy.spin).
Twist: standard ROS 2 message for robot velocity (linear + angular).
sys, termios, tty, select, os: used for low-level keyboard input in the terminal.

2. Keyboard Handling

def has_tty():
    return os.isatty(sys.stdin.fileno())

Checks if there’s a real terminal (TTY). If running in a non-interactive environment (like a launch file or Docker container without keyboard), it disables keyboard control.

def get_key_nonblocking():
    if not has_tty():
        return None   # no keyboard available
    
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(fd)   # set terminal to raw mode (no buffering, no echo)
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        rlist, _, _ = select.select([sys.stdin], [], [], 0)  # non-blocking check
        if rlist:
            return sys.stdin.read(1)   # read a single key
        return None
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)

This makes keyboard reading non-blocking:
- Doesn’t freeze waiting for input.
- Reads one character if available, otherwise returns None.

3. The Teleop Node

@node("teleop")
class Teleop:
    def __init__(self):
        self.twist = Twist()
        self.logger.info("Teleop started. Use WASD to move, space to stop, 'q' to quit.")

Declares a ROS 2 node named "teleop".
Creates a Twist message object (self.twist) to reuse each loop.
Logs startup instructions.

4. Publishing Loop

@timer(period_sec=0.1)  # 10 Hz loop
@publisher(topic="cmd_vel", msg_type=Twist)
def publish_cmd(self):
    key = get_key_nonblocking()

    # reset to zero each cycle unless overridden
    self.twist.linear.x = 0.0
    self.twist.angular.z = 0.0

This method runs every 0.1s (10 Hz).
It reads the last pressed key.
By default, velocity is reset to 0 every cycle (so robot stops unless a key is actively pressed).

    if key == 'w':
        self.twist.linear.x = 1.5
    elif key == 's':
        self.twist.linear.x = -1.5
    elif key == 'a':
        self.twist.angular.z = 1.5
    elif key == 'd':
        self.twist.angular.z = -1.5
    elif key == 'q':
        self.logger.info("Quitting teleop…")
        raise SystemExit
    # space already handled by reset-to-zero above

Controls:
- w → forward (+x velocity)
- s → backward (−x velocity)
- a → rotate left (+z angular)
- d → rotate right (−z angular)
- space → stop (handled by the reset-to-zero step)
- q → quit program
1.5 is the chosen speed/turn rate.

Finally:

    return self.twist

Returning the Twist automatically publishes it to /cmd_vel (because of the @publisher decorator).

5. Entry Point

def main(args=None):
    spin_node(Teleop, args)

if __name__ == "__main__":
    main()

spin_node runs the Teleop class as a Genesys node, handling timers, publishers, and shutdown.

🔑 In Plain English

This code is a keyboard teleoperation controller for the robot in ROS 2 (via Genesys).

It listens to keyboard keys (WASD, space, q).
Every 0.1s, it publishes a Twist message on /cmd_vel.
The robot moves forward, backward, or rotates depending on the key pressed.
If no key is pressed, it automatically stops.
Press q to quit.

After you have written the node logic, build the workspace

genesys build

Then open another terminal. In the first terminal, run the sim command

genesys sim empty.world

This should spin up the gazebo simulator and spawn your robot in the empty world. So you should be able to see your robot in an empty world on the gazebo simulator. See how easy that was!!!?

Next, in the second terminal, run the node

genesys run teleop_pkg_node

And just like that, you have a robot you can control with your keyboard. Wasn't that fun?!

Use the [WASD] keys to control your robot. Always make sure that you are on the second terminal when you are using your keyboard to control the robot

Congratulations! You just made your first robot!

PreviousGenesys vs ROS2

Last updated 1 month ago

Was this helpful?