Driving a robot in simulation is easy. Bringing it into the real world? That's where the fun — and the headaches — begin.
If you're building a custom 2WD differential drive robot using the Yahboom 4-Channel Encoder Motor Driver Board and 520 DC Motors, you'll quickly hit a classic hardware challenge: how do you send velocity commands (cmd_vel) and read back encoder data (odom) over a single serial pipeline without crashing your nodes?
In this post, we'll cover how the Yahboom board handles raw data packets, why multi-node serial setups fail, and how to write a unified ROS 2 Python bridge that solves the whole problem cleanly.
How the Yahboom 4-Channel Driver Actually Works
Unlike passive motor drivers (like an L298N) that require your main processor to count high-frequency square waves via hardware interrupts, the Yahboom 4-Channel Board acts as an intelligent coprocessor. Its onboard STM32F103RCT6 microcontroller handles all the time-critical work: reading Hall-effect encoders, running PID control loops, and managing state changes.
Because the STM32 handles the heavy lifting, your primary processor (Jetson Orin Nano, Raspberry Pi, etc.) only needs to talk to it via simple async strings over UART Serial (or I2C, depending on your board's dip-switch config).
Decoding the Serial Protocol
Every message uses clear start/stop sentinels to prevent corruption:
-
$(Header) — tells the receive buffer to start collecting characters. Everything before this is ignored. -
#(Footer) — signals the payload is complete and ready to parse.
1. Motor Type Configuration — $mtype:x#
Before sending any speed commands, you must declare your motor profile so the STM32 applies the correct gear ratios and PID settings.
$mtype:1#
This sets the driver explicitly for Yahboom 520 encoder motors.
2. Writing Speed — $speed:A,B,C,D#
Speed commands pass a comma-separated array of four integer values, one per channel. The accepted range is -1000 to 1000 — positive is forward, negative is backward.
For a 2WD chassis, motors map to Channel A (Left) and Channel C (Right). Channels B and D stay pinned to 0.
$speed:500,0,500,0# # Move forward at half speed
3. Reading Encoders — $read# → $data:...#
To calculate odometry, poll the board by sending:
$read#
The board immediately responds with cumulative pulse counts:
$data:A_pulses,B_pulses,C_pulses,D_pulses#
Your ROS 2 node strips the tags and extracts indices 0 (left) and 2 (right) for wheel odometry calculations.
The Pitfall: The Serial "Resource Busy" Error
The natural instinct in ROS 2 is to keep things modular:
- An Odometry Node that polls encoder ticks with
$read# - A Velocity Node that listens to
/cmd_veland writes$speed:A,0,C,0#
If you run these as two separate processes targeting the same port (e.g., /dev/ttyUSB0), Linux throws this immediately:
SerialException: [Errno 16] Device or resource busy.
Linux prevents multiple processes from binding to the same serial device to avoid data corruption. The fix: a Unified Bridge Node that owns a single shared file handle and handles both reading and writing asynchronously.
The Architecture: Closed-Loop Control
Here's how the unified node sits between Nav2 and your physical wheels:
[ Nav2 Path Planner / Teleop ]
│
▼ (Publishes target speed)
Topic: /cmd_vel
│
▼
┌────────────────────────────────────────┐
│ Yahboom Unified Robot Bridge │
│ ──────────────────────────────────── │
│ ► Subscribes to /cmd_vel ──────────► Writes $speed:Left,0,Right,0#
│ ► Main Loop Timer (20Hz) ◄────────── Reads $data:A,B,C,D# Telemetry
└────────────────────────────────────────┘
│
▼ (Publishes local coordinates)
Topic: /odom & /tf
│
▼
[ Nav2 Path Planner / RViz2 ]
The Code: yahboom_robot_bridge.py
This script is your complete hardware driver. It initializes the coprocessor, reads encoder feedback at 20 Hz, keeps the odometry frame updated, and handles incoming velocity commands immediately.
import rclpy
from rclpy.node import Node
import serial
import math
import time
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped, Quaternion, Twist
import tf2_ros
class YahboomRobotBridge(Node):
def __init__(self):
super().__init__('yahboom_robot_bridge')
# 1. Unified Serial Port Connection — opened ONCE
self.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.05)
self.ser.write(b"$mtype:1#") # Configure for 520 motor profile
time.sleep(0.1)
self.ser.reset_input_buffer()
# 2. Physical Robot Parameters — adjust to match your chassis
self.WHEEL_BASE = 0.150 # Distance between wheels (meters)
self.WHEEL_RADIUS = 0.0325 # Wheel radius (meters)
self.TICKS_PER_REV = 1320.0 # 520 motor output ticks per revolution
self.SCALE_FACTOR = 1000.0 # Maps m/s → raw motor range (-1000 to 1000)
# 3. Odometry state
self.x = 0.0
self.y = 0.0
self.th = 0.0 # Heading in radians
self.prev_left_ticks = 0
self.prev_right_ticks = 0
self.last_time = self.get_clock().now()
# 4. ROS 2 interfaces
self.odom_pub = self.create_publisher(Odometry, '/odom', 10)
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
self.cmd_vel_sub = self.create_subscription(
Twist, '/cmd_vel', self.cmd_vel_callback, 10
)
# 5. Odometry timer — 20 Hz
self.odom_timer = self.create_timer(0.05, self.update_and_publish_odom)
self.get_logger().info("Unified Yahboom Robot Bridge started.")
def euler_to_quaternion(self, roll, pitch, yaw):
qx = math.sin(roll/2)*math.cos(pitch/2)*math.cos(yaw/2) - math.cos(roll/2)*math.sin(pitch/2)*math.sin(yaw/2)
qy = math.cos(roll/2)*math.sin(pitch/2)*math.cos(yaw/2) + math.sin(roll/2)*math.cos(pitch/2)*math.sin(yaw/2)
qz = math.cos(roll/2)*math.cos(pitch/2)*math.sin(yaw/2) - math.sin(roll/2)*math.sin(pitch/2)*math.cos(yaw/2)
qw = math.cos(roll/2)*math.cos(pitch/2)*math.cos(yaw/2) + math.sin(roll/2)*math.sin(pitch/2)*math.sin(yaw/2)
return Quaternion(x=qx, y=qy, z=qz, w=qw)
def cmd_vel_callback(self, msg):
"""Convert Nav2 velocity commands → motor speed and write to serial."""
linear_x = msg.linear.x
angular_z = msg.angular.z
# Inverse differential kinematics
left_vel = linear_x - (angular_z * self.WHEEL_BASE / 2.0)
right_vel = linear_x + (angular_z * self.WHEEL_BASE / 2.0)
# Scale to raw motor range
left_cmd = max(min(int(left_vel * self.SCALE_FACTOR), 1000), -1000)
right_cmd = max(min(int(right_vel * self.SCALE_FACTOR), 1000), -1000)
# Channel A = Left, Channel C = Right, B & D locked to 0
command = f"$speed:{left_cmd},0,{right_cmd},0#"
try:
self.ser.write(command.encode('utf-8'))
except Exception as e:
self.get_logger().error(f"Write error: {str(e)}")
def update_and_publish_odom(self):
"""Poll encoders, integrate position, publish /odom and /tf."""
current_time = self.get_clock().now()
dt = (current_time - self.last_time).nanoseconds / 1e9
if dt <= 0.0:
return
try:
self.ser.write(b"$read#")
response = self.ser.readline().decode('utf-8').strip()
if response.startswith("$data:") and response.endswith("#"):
clean_data = response.replace("$data:", "").replace("#", "")
ticks = clean_data.split(",")
current_left = int(ticks[0])
current_right = int(ticks[2])
delta_left = current_left - self.prev_left_ticks
delta_right = current_right - self.prev_right_ticks
self.prev_left_ticks = current_left
self.prev_right_ticks = current_right
# Ticks → meters
meters_per_tick = (2 * math.pi * self.WHEEL_RADIUS) / self.TICKS_PER_REV
dist_left = delta_left * meters_per_tick
dist_right = delta_right * meters_per_tick
delta_dist = (dist_left + dist_right) / 2.0
delta_th = (dist_right - dist_left) / self.WHEEL_BASE
vx = delta_dist / dt
vth = delta_th / dt
# Integrate global position
self.x += delta_dist * math.cos(self.th + (delta_th / 2.0))
self.y += delta_dist * math.sin(self.th + (delta_th / 2.0))
self.th += delta_th
odom_quat = self.euler_to_quaternion(0.0, 0.0, self.th)
# Broadcast /odom → /base_link transform
t = TransformStamped()
t.header.stamp = current_time.to_msg()
t.header.frame_id = 'odom'
t.child_frame_id = 'base_link'
t.transform.translation.x = self.x
t.transform.translation.y = self.y
t.transform.translation.z = 0.0
t.transform.rotation = odom_quat
self.tf_broadcaster.sendTransform(t)
# Publish /odom message
odom = Odometry()
odom.header.stamp = current_time.to_msg()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.orientation = odom_quat
odom.twist.twist.linear.x = vx
odom.twist.twist.angular.z = vth
self.odom_pub.publish(odom)
self.last_time = current_time
except Exception as e:
self.get_logger().error(f"Read error: {str(e)}")
def destroy_node(self):
"""Safe shutdown — stop motors before closing serial."""
self.get_logger().info("Stopping robot wheels...")
try:
self.ser.write(b"$speed:0,0,0,0#")
self.ser.close()
except Exception:
pass
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
node = YahboomRobotBridge()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
The Math Behind the Code
Two kinematic concepts drive the odometry loop.
1. Inverse Kinematics — /cmd_vel → Wheel Speeds
When Nav2 commands a rotation, it passes an angular velocity (angular.z). To turn left, the left wheel slows or reverses while the right wheel accelerates. The track width scales how much each side adjusts:
$$V_{left} = V_{linear} - \left(\Omega_{angular} \times \frac{W_{base}}{2}\right)$$
$$V_{right} = V_{linear} + \left(\Omega_{angular} \times \frac{W_{base}}{2}\right)$$
2. Forward Kinematics — Ticks → Global Position
Each odometry cycle computes how far each wheel traveled during a small time slice dt. The current heading θ decomposes that distance into X and Y components on the map:
$$\Delta X = \Delta dist \times \cos(\theta)$$
$$\Delta Y = \Delta dist \times \sin(\theta)$$
$$\Delta\theta = \frac{dist_{right} - dist_{left}}{W_{base}}$$
Deployment Tips
Calibrate SCALE_FACTOR on your actual surface. Place the robot on the floor, command it to drive exactly 1 meter via a teleop script, and check if RViz2 shows x = 1.0. Carpet, tile, and hardwood will all give different results — adjust accordingly.
Use a persistent udev rule. If you have a LiDAR plugged in alongside the driver board, the port assignments (/dev/ttyUSB0 vs /dev/ttyUSB1) can swap on reboot. Create a udev rule to lock your driver board to a static path like /dev/yahboom_driver:
# /etc/udev/rules.d/99-yahboom.rules
SUBSYSTEM=="tty", ATTRS{idVendor}=="xxxx", ATTRS{idProduct}=="xxxx", SYMLINK+="yahboom_driver"
Replace the vendor/product IDs with the output of udevadm info -a /dev/ttyUSB0 | grep idVendor.
Happy building! Drop a comment if you're adapting this for a 4WD Mecanum chassis — the channel mapping and kinematic equations change, but the unified serial architecture stays exactly the same.