Sensing · Chapter 14 · Time: 2.5 hours · Level: Intermediate · Status: Done on this robot
The COIN-D6 LiDAR on /scan at 10 Hz - its undocumented serial packets decoded and checksummed in coin_d6.py, a read-only reader node that survives USB drops and its own crashes, the right orientation and timestamps, and the numbers that say a scan is healthy.
The COIN-D6 on top of the robot spins about ten times a second and measures the distance to whatever is around it,
roughly 400 times per turn, in one horizontal plane. It is the sensor that maps the room, places the robot on the
map and keeps it off the walls, and the base driver will not move the wheels on a software command unless scans
keep arriving. No datasheet for it was found; its serial protocol was worked out from captured bytes on 2026-09-26.
This chapter rebuilds that work: two small Python files, about 225 lines, that read the port without ever writing
to it.
New idea: a 2D LiDAR and the LaserScan message
A 2D LiDAR turns a range-finder in a circle and reports (angle, distance) pairs. ROS publishes one turn as a
sensor_msgs/LaserScan:angle_min,angle_incrementand arangesarray (metres, one value per angle step,
infwhere nothing came back), plusrange_min/range_max(values outside are to be ignored),
intensities,scan_time(seconds per turn),time_increment(seconds between two rays) and the usual
headerwith a time stamp and the frame the angles are measured in (herelaser, chapter 12). Angles grow
counter-clockwise, like everywhere in ROS: 0 straight ahead along the frame's x axis, +90 degrees to its left.
New idea: a sensor time stamp
Every measurement carries the time it was taken. Consumers (localization, costmaps) look up the robot's pose
from TF at that time and place the points with it. A stamp that is too new makes them wait for transforms
that do not exist yet; a stamp that is too old places the points where the robot used to be. For a LaserScan the
convention is the time of the first ray.
ch341 module loads, and the udev rule /etc/udev/rules.d/60-rosorin-lidar.rules names therosorin_base package from chapters 10-12 builds on the robot.The rule file holds one line:
On the robot:
SUBSYSTEM=="tty", KERNELS=="1-2.2.4:1.0", SYMLINK+="lidar"
It matches the USB path, not the device name, because the voice box on this robot also had a CH340 serial chip, and
the ttyUSB0 / ttyUSB1 numbers swap when a device re-enumerates (2026-09-28: the LiDAR went from ttyUSB1 to
ttyUSB0).
On the robot:
ls -l /dev/lidar
Check
Recorded after the reboot test of 2026-09-26:lrwxrwxrwx 1 root root 7 Sep 26 23:31 /dev/lidar -> ttyUSB0
If it fails
- No
/dev/lidarand nottyUSB*at all: the CH340 has no driver. The stock R36.4.3 kernel has
CONFIG_USB_SERIAL_CH341off, and the module built in chapter 7 is tied to the exact kernel build. After any
L4T kernel update rebuild it (scripts/build_ch341.sh+scripts/install_ch341.sh) or the LiDAR has no tty.brlttywas purged on 2026-09-26 because it flooded the journal. It was not the reason the LiDAR had no tty.
The COIN-D6 starts sending as soon as it has power, at 230400 baud, with no start command (the baud rate is the one
the factory sclidar.launch.py used). The reader opens the port exclusively, so if the base service already runs
the LiDAR reader (it does on the finished robot), stop the service first.
Safety
Stoppingrosorin-basestops the board driver: the wheels get the stop frame, and the arm stays where it is with
its servos holding.rosorin-nav(chapter 18) requires the base and stops with it;rosorin-mind, if installed,
must be restarted after the base is back (chapter 12, Step 4). Do this with the wheels disabled and nobody
relying on the robot.
On the robot:
sudo systemctl stop rosorin-base
stty -F /dev/lidar 230400 raw -echo && timeout 2 cat /dev/lidar > /tmp/lidar.bin
stat -c "%s bytes in 2s" /tmp/lidar.bin
xxd /tmp/lidar.bin | head -4
Check
Recorded on 2026-09-26 (first read, onttyUSB0):27506 bytes in 2s 00000000: aa55 0019 4f43 214e b613 8449 35f8 a535 .U..OC!N...I5..5 00000010: bccd 3540 dd27 d0ed 27b8 dc36 3c11 3718 ..5@.'..'..6<.7. 00000020: 6e37 c0a6 2f94 af2f 784a 2f50 b938 ccb4 n7../../xJ/P.8.. 00000030: 3930 e939 7070 4b6c 1031 a0b0 3480 5a26 90.9ppKl.1..4.Z&About 13.7 KB/s, and the bytes
aa 55start the stream. After the reboot test the same count over 2 s was 27599.
Start the base again when you are done with the raw bytes (or after Step 6):sudo systemctl start rosorin-base.
If it fails
sttyorcatfails with "Device or resource busy" (EBUSY): another process holds the port. Our readers
open it withTIOCEXCL, so a second opener is refused. That refusal is on purpose: two readers on one serial
port silently split the bytes between them (seen on the controller board, where the IMU looked like 60 Hz
instead of 110). Stop the other reader.- 0 bytes: wrong device, or the LiDAR has no power. Check
ls -l /dev/lidarand that the top spins.
The stream is a sequence of packets. This is the layout as established from the data (docs/hardware.md, verified
2026-09-26); it matches the YDLIDAR TOF "intensity" packet:
| Bytes | Name | Meaning |
|---|---|---|
| 0-1 | PH | header AA 55 |
| 2 | CT | bit 0 = 1: this packet starts a new revolution (it then carries 1 sample) |
| 3 | LSN | number of samples in the packet (25 in a normal packet) |
| 4-5 | FSA | angle of the first sample: (u16 >> 1) / 64 degrees |
| 6-7 | LSA | angle of the last sample, same encoding; samples in between are spaced evenly |
| 8-9 | CS | checksum: XOR of the 16-bit words PH, CT/LSN, FSA, LSA, every S0, and every (S2 << 8 | S1) |
| 10 ... | samples | 3 bytes each: S0 = intensity, distance mm = (S2 << 6) | (S1 >> 2); the low 2 bits of S1 are unknown and ignored |
All 16-bit values are little-endian. A normal packet from the repo's capture test/coin_d6_sample.bin:
Board bytes:
aa 55 | 00 | 19 | 49 16 | 03 21 | 84 7b | 10 4e 01 | ...
PH CT LSN FSA LSA CS S0 S1 S2
49 16 = 0x1649 = 5705; 5705 >> 1 = 2852; 2852 / 64 = 44.5625 degrees. LSA: 0x2103 >> 1 = 4225;A revolution-start packet from the same capture, with its checksum worked through:
Board bytes:
aa 55 c9 01 7b b3 7b b3 2c 54 4c 03 00
CT 0xC9 has bit 0 set; LSN 1; FSA = LSA = 0xb37b, 358.95 degrees; one sample with intensity 0x4c and distance 0 (no
return). The checksum (computed with the repo's coin_d6.py for this guide):
Board bytes:
0x55aa PH (bytes aa 55, little-endian)
^ 0x01c9 CT | LSN << 8 -> 0x5463
^ 0xb37b FSA -> 0xe718
^ 0xb37b LSA -> 0x5463
^ 0x004c S0 -> 0x542f
^ 0x0003 S2 << 8 | S1 -> 0x542c = CS (bytes 2c 54)
How the format was found (docs/lessons.md 2026-09-26), from a 3 s capture: first the length rule (the gap between
headers equals 10 + 3 x LSN for 524 of 526 headers), then checksum candidates (only the YDLIDAR "intensity" XOR
matched, 524 of 524), then the distance encoding by physical sense:
bytes 41399 headers 527 top gaps [(85, 477), (13, 47), (32, 1), (96, 1)]
CT byte [(0, 498), (201, 29)] LSN byte [(25, 480), (1, 47)]
layout matches {'10+3n': 524} of 526
ydl_intensity: ^S0, ^(S2<<8|S1) 524/524
words over samples 0/524
ydl_2byte words (S1,S2) 10/524
TOF mm: n=392 min=1 median=6799 max=31749
TRI mm: n=303 min=51 median=2581 max=7937
Reading the distance as a plain 16-bit number ("TOF") gave 3 mm points and a 31.7 m maximum in a living room:
nonsense. The (S2 << 6) | (S1 >> 2) reading ("TRI") gave 51 mm to 7.94 m, and per-degree values over 28 revolutions
of a static scene varied by a median of 6 mm. The two aa 55 headers that did not fit the length rule are the
reason the parser below must check every candidate packet: aa 55 also occurs inside sample bytes.
Create ~/ros2_ws/src/rosorin_base/rosorin_base/coin_d6.py. It knows nothing about ROS or serial ports: bytes in,
packets and points out. Start with the description and a helper that reads a little-endian 16-bit word:
On the robot:
"""COIN-D6 LiDAR serial protocol, as verified on this robot (docs/hardware.md, 2026-09-26).
Matches the YDLIDAR TOF 'intensity' packet layout (checksum verified 524/524):
0-1 PH 0xAA 0x55
2 CT bit0 = ring-start packet
3 LSN sample count
4-5 FSA first angle, (u16 >> 1) / 64 deg
6-7 LSA last angle
8-9 CS XOR of u16 words: PH, CT|LSN, FSA, LSA, each sample S0 and (S2<<8|S1)
10.. samples, 3 bytes: S0 intensity, distance mm = (S2 << 6) | (S1 >> 2)
Low 2 bits of S1: meaning unverified, ignored.
"""
def _w(b, i):
return b[i] | (b[i + 1] << 8)
The checksum, exactly as in the table: the four header words, then for each sample the intensity byte alone and the
two distance bytes as one word:
On the robot:
def checksum_ok(pkt: bytes) -> bool:
n = pkt[3]
x = _w(pkt, 0) ^ _w(pkt, 2) ^ _w(pkt, 4) ^ _w(pkt, 6)
for k in range(n):
o = 10 + 3 * k
x ^= pkt[o]
x ^= _w(pkt, o + 1)
return x == _w(pkt, 8)
Decoding a packet into (angle, distance, intensity) points. The % 360.0 on the span handles a packet that crosses
from 359 to 0 degrees:
On the robot:
def decode(pkt: bytes):
"""-> (ring_start, [(angle_deg, distance_mm, intensity), ...])"""
n = pkt[3]
fsa = (_w(pkt, 4) >> 1) / 64.0
lsa = (_w(pkt, 6) >> 1) / 64.0
span = (lsa - fsa) % 360.0
pts = []
for k in range(n):
o = 10 + 3 * k
s0, s1, s2 = pkt[o], pkt[o + 1], pkt[o + 2]
a = (fsa + span * k / (n - 1)) % 360.0 if n > 1 else fsa
pts.append((a, (s2 << 6) | (s1 >> 2), s0))
return bool(pkt[2] & 1), pts
The parser. Serial reads arrive in arbitrary chunks, so it keeps a buffer, looks for aa 55, waits until the whole
packet is there, and keeps the packet only if the checksum is right. On a bad checksum it drops the two header bytes
and searches again ("resync by 2"): a false header inside a sample must not swallow the real packet behind it.
On the robot:
class PacketParser:
def __init__(self):
self.buf = bytearray()
self.bad = 0
def feed(self, data: bytes):
self.buf += data
out = []
while True:
i = self.buf.find(b'\xaa\x55')
if i < 0:
del self.buf[:-1]
return out
if i:
del self.buf[:i]
if len(self.buf) < 10:
return out
size = 10 + 3 * self.buf[3]
if len(self.buf) < size:
return out
pkt = bytes(self.buf[:size])
if checksum_ok(pkt):
out.append(pkt)
del self.buf[:size]
else:
self.bad += 1
del self.buf[:2]
del self.buf[:-1] when no header is found keeps the last byte: it may be the aa of a header whose 55 has not
arrived yet. The complete file (70 lines, ros2/rosorin_base/rosorin_base/coin_d6.py):
On the robot:
"""COIN-D6 LiDAR serial protocol, as verified on this robot (docs/hardware.md, 2026-09-26).
Matches the YDLIDAR TOF 'intensity' packet layout (checksum verified 524/524):
0-1 PH 0xAA 0x55
2 CT bit0 = ring-start packet
3 LSN sample count
4-5 FSA first angle, (u16 >> 1) / 64 deg
6-7 LSA last angle
8-9 CS XOR of u16 words: PH, CT|LSN, FSA, LSA, each sample S0 and (S2<<8|S1)
10.. samples, 3 bytes: S0 intensity, distance mm = (S2 << 6) | (S1 >> 2)
Low 2 bits of S1: meaning unverified, ignored.
"""
def _w(b, i):
return b[i] | (b[i + 1] << 8)
def checksum_ok(pkt: bytes) -> bool:
n = pkt[3]
x = _w(pkt, 0) ^ _w(pkt, 2) ^ _w(pkt, 4) ^ _w(pkt, 6)
for k in range(n):
o = 10 + 3 * k
x ^= pkt[o]
x ^= _w(pkt, o + 1)
return x == _w(pkt, 8)
def decode(pkt: bytes):
"""-> (ring_start, [(angle_deg, distance_mm, intensity), ...])"""
n = pkt[3]
fsa = (_w(pkt, 4) >> 1) / 64.0
lsa = (_w(pkt, 6) >> 1) / 64.0
span = (lsa - fsa) % 360.0
pts = []
for k in range(n):
o = 10 + 3 * k
s0, s1, s2 = pkt[o], pkt[o + 1], pkt[o + 2]
a = (fsa + span * k / (n - 1)) % 360.0 if n > 1 else fsa
pts.append((a, (s2 << 6) | (s1 >> 2), s0))
return bool(pkt[2] & 1), pts
class PacketParser:
def __init__(self):
self.buf = bytearray()
self.bad = 0
def feed(self, data: bytes):
self.buf += data
out = []
while True:
i = self.buf.find(b'\xaa\x55')
if i < 0:
del self.buf[:-1]
return out
if i:
del self.buf[:i]
if len(self.buf) < 10:
return out
size = 10 + 3 * self.buf[3]
if len(self.buf) < size:
return out
pkt = bytes(self.buf[:size])
if checksum_ok(pkt):
out.append(pkt)
del self.buf[:size]
else:
self.bad += 1
del self.buf[:2]
test/coin_d6_sample.bin is 3000 bytes of the real stream captured on the robot on 2026-09-26. It starts in the
middle of a packet, as any capture does. Copy it and the test file from the repo:
On your laptop:
ssh rosorin 'mkdir -p ~/ros2_ws/src/rosorin_base/test'
scp ~/CCode/rosorin-pro/ros2/rosorin_base/test/coin_d6_sample.bin ~/CCode/rosorin-pro/ros2/rosorin_base/test/test_coin_d6.py rosorin:ros2_ws/src/rosorin_base/test/
The test file (ros2/rosorin_base/test/test_coin_d6.py):
On the robot:
import os
from rosorin_base.coin_d6 import PacketParser, checksum_ok, decode
SAMPLE = os.path.join(os.path.dirname(__file__), 'coin_d6_sample.bin') # real capture 2026-09-26
def test_real_capture_all_checksums_ok():
p = PacketParser()
pkts = p.feed(open(SAMPLE, 'rb').read())
# The capture starts mid-packet and 0xAA 0x55 can occur inside payload bytes, so the parser may
# hit a false header; it must reject it by checksum and resync, never return a bad packet.
assert len(pkts) >= 30 and p.bad <= 2
assert all(checksum_ok(x) for x in pkts)
def test_decode_shape():
pkts = PacketParser().feed(open(SAMPLE, 'rb').read())
normal = [decode(x) for x in pkts if x[3] == 25]
assert normal and all(len(pts) == 25 for _, pts in normal)
for _, pts in normal:
assert all(0 <= a < 360 and 0 <= mm <= 16383 for a, mm, _ in pts)
def test_corrupted_packet_rejected():
pkts = PacketParser().feed(open(SAMPLE, 'rb').read())
bad = bytearray(pkts[1]); bad[12] ^= 0x40
p = PacketParser()
assert p.feed(bytes(bad)) == [] and p.bad == 1
On the robot:
cd ~/ros2_ws/src/rosorin_base
python3 -m pytest -q test/test_coin_d6.py
python3 -c "
from rosorin_base.coin_d6 import PacketParser
p=PacketParser(); d=open(\"test/coin_d6_sample.bin\",\"rb\").read(); k=p.feed(d)
print(\"pkts\",len(k),\"bad\",p.bad,\"first aa55 at\",d.find(bytes([0xaa,0x55])),\"leftover\",len(p.buf))"
Check
pytest ends with3 passed. The one-liner, as recorded on the robot on 2026-09-26:pkts 37 bad 1 first aa55 at 30 leftover 937 good packets (34 normal ones of 25 samples and 3 revolution starts), one false header rejected, 30 bytes of a
cut-off packet skipped at the start, 9 bytes of an unfinished packet left in the buffer at the end.
If it fails
The first version of this test assertedp.bad == 0and failed on the robot (2026-09-26):E assert (37 >= 30 and 1 == 0)The parser was right; the test was wrong. The capture contains an
aa 55that is not a header, the checksum
rejected it, and the parser resynced. Never assume the first header you find is real (docs/lessons.md).
The node turns packets into one LaserScan per revolution. Create
~/ros2_ws/src/rosorin_base/rosorin_base/lidar_reader.py. It is 155 lines; build it in seven pieces.
Imports and the number of angle bins. The LiDAR delivers about 401 samples per turn (16 packets of 25 plus the start
packet's one), so 400 bins of 0.9 degrees hold one sample each:
On the robot:
"""Read-only COIN-D6 LiDAR -> sensor_msgs/LaserScan. Opens the port O_RDONLY; sends nothing."""
import fcntl
import math
import os
import select
import termios
import threading
import time
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.duration import Duration
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rosorin_base.coin_d6 import PacketParser, decode
BINS = 400 # 0.9 deg, matches the measured ~401 samples/ring
The node's parameters and its reader thread. clockwise defaults to False here and is set True by the launch file
(Step 8). range_min 0.12 m hides the self-returns (at most 106 mm):
On the robot:
class LidarReader(Node):
def __init__(self):
super().__init__('rosorin_lidar_reader')
self.dev = self.declare_parameter('device', '/dev/lidar').value
self.frame_id = self.declare_parameter('frame_id', 'laser').value
# Native angles run clockwise: launch sets clockwise=True (floor test 2026-09-28, docs/hardware.md)
self.offset = self.declare_parameter('angle_offset_deg', 0.0).value
self.clockwise = self.declare_parameter('clockwise', False).value
self.range_min = self.declare_parameter('range_min', 0.12).value # masks self-returns (<=106 mm)
self.range_max = self.declare_parameter('range_max', 8.0).value
self.pub = self.create_publisher(LaserScan, 'scan', 10)
self.parser = PacketParser()
self.stale_s = self.declare_parameter('reopen_after_silence_s', 2.0).value
self.fd = self.open_port()
self.ring = None
self.ring_t = None
self.running = True
self.thread = threading.Thread(target=self.loop, daemon=True)
self.thread.start()
self.get_logger().info(f'reading {self.dev} (read-only)')
Opening the port: read-only (O_RDONLY, the node cannot send anything to the LiDAR), exclusive (TIOCEXCL), raw 8
bits at 230400 baud, and read returns as soon as one byte is there (VMIN 1, VTIME 0). This is the same raw
setup as the controller board's port in chapter 10, at a different speed:
On the robot:
def open_port(self):
fd = os.open(self.dev, os.O_RDONLY | os.O_NOCTTY)
fcntl.ioctl(fd, termios.TIOCEXCL)
a = termios.tcgetattr(fd)
a[0] = a[1] = a[3] = 0
a[2] = termios.CS8 | termios.CREAD | termios.CLOCAL
a[4] = a[5] = termios.B230400
a[6][termios.VMIN] = 1
a[6][termios.VTIME] = 0
termios.tcsetattr(fd, termios.TCSANOW, a)
return fd
Reopening. On 2026-09-28 at 05:52 the LiDAR's USB device dropped and came back while the robot was handled
(EPIPE, ttyUSB1 became ttyUSB0). The reader kept its old file descriptor, which never delivered another byte,
and /scan stayed silent until a restart. Now it closes the dead descriptor, starts a fresh parser, and retries the
open every 0.5 s until /dev/lidar is back:
On the robot:
def reopen(self, why):
"""USB re-enumeration (seen 2026-09-28: EPIPE, ttyUSB1 -> ttyUSB0) leaves the old fd dead."""
self.get_logger().warn(f'{self.dev}: {why}; reopening')
try:
os.close(self.fd)
except OSError:
pass
self.fd, self.ring, self.parser = None, None, PacketParser()
while self.running:
try:
self.fd = self.open_port()
self.get_logger().info(f'{self.dev} reopened')
return
except OSError:
time.sleep(0.5)
The reader loop runs in its own thread. It waits up to 0.2 s for bytes and reopens on a read error, on end of file
(the device is gone) or after 2 s without any byte. Packets go into the current revolution until the next
revolution-start packet arrives; then the finished revolution is published with its duration:
On the robot:
def loop(self):
last_data = time.monotonic()
while self.running:
try:
ready, _, _ = select.select([self.fd], [], [], 0.2)
data = os.read(self.fd, 1024) if ready else None
except OSError as e:
self.reopen(f'read error {e}')
last_data = time.monotonic()
continue
if data == b'':
self.reopen('EOF (device gone)')
last_data = time.monotonic()
continue
if not data:
if time.monotonic() - last_data > self.stale_s:
self.reopen(f'no data for {self.stale_s:.0f} s')
last_data = time.monotonic()
continue
last_data = time.monotonic()
try:
for pkt in self.parser.feed(data):
start, pts = decode(pkt)
if start:
now = time.monotonic()
if self.ring:
self.publish(self.ring, now - self.ring_t)
self.ring, self.ring_t = [], now
elif self.ring is not None:
self.ring.extend(pts)
except Exception as e:
if not rclpy.ok():
return
# a dead reader thread would leave the node up and silent for good: exit, launch respawns the node
# (base.launch.py respawn=True); meanwhile the base driver's scan gate holds the wheels
self.get_logger().error(f'reader thread failed: {e!r}; exiting for a restart')
os._exit(1)
The except Exception at the bottom handles the worst case: a bug or a surprise in the data kills the thread. A
node whose thread is dead stays "running" and publishes nothing, forever; the factory stack had exactly that problem
("drivers can stall silently - process alive, zero messages", docs/lessons.md). So the node exits the whole
process (os._exit(1)), and the launch file starts a new one. The rclpy.ok() test keeps a normal Ctrl-C from
looking like a crash: rclpy's SIGINT handler invalidates the context first, and threads must take that as "stop".
New idea: clockwise native angles
The COIN-D6 numbers its angles clockwise seen from above. ROS angles run counter-clockwise. So the reader
negates the native angle whenclockwiseis set. It does not rotate the scan to the robot's forward: the
LiDAR's 180 degrees points forward, and the static transformbase_link -> laser(yaw pi, chapter 12) handles
that. Published angle 0 in framelaseris the LiDAR's own 0.
Publishing one revolution. Each point goes into the bin of its (negated) angle; distance 0 means no return and is
skipped; if two samples land in one bin, the nearer one wins. Empty bins stay inf:
On the robot:
def publish(self, pts, dt):
ranges = [float('inf')] * BINS
inten = [0.0] * BINS
for a, mm, i in pts:
if mm == 0:
continue
ang = (-a if self.clockwise else a) + self.offset
b = int(round((ang % 360.0) / 360.0 * BINS)) % BINS
r = mm / 1000.0
if r < ranges[b]:
ranges[b] = r
inten[b] = float(i)
m = LaserScan()
# REP/LaserScan: stamp = first ray. Ring is published when the next ring starts, so the first
# ray was dt ago. (Stamping 'now' put every scan ~100 ms ahead of TF: AMCL/costmap dropped scans,
# Nav2 collision 2026-09-28.)
m.header.stamp = (self.get_clock().now() - Duration(seconds=dt)).to_msg()
m.header.frame_id = self.frame_id
m.angle_min = 0.0
m.angle_increment = 2 * math.pi / BINS
m.angle_max = m.angle_increment * (BINS - 1)
m.scan_time = dt
m.time_increment = dt / BINS
m.range_min = self.range_min
m.range_max = self.range_max
m.ranges = ranges
m.intensities = inten
self.pub.publish(m)
The stamp line is the fix for the 2026-09-28 collision. A revolution is published when the next one starts, so its
first ray was taken dt (about 0.1 s) earlier. The first version stamped "now": every scan looked 100 ms newer than
the newest transform, AMCL dropped them all, the map-to-odom transform went 10 s stale, and the robot drove about
2.6 m past a 0.6 m goal into objects (no damage, docs/navigation.md). After the fix: map-to-odom at 10 Hz and no
"Transform data too old" messages.
Shutdown and the entry point:
On the robot:
def stop(self):
self.running = False
self.thread.join(timeout=1.0)
if self.fd is not None:
os.close(self.fd)
def main():
rclpy.init()
node = LidarReader()
try:
rclpy.spin(node)
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
node.stop()
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
The complete file (155 lines, ros2/rosorin_base/rosorin_base/lidar_reader.py):
On the robot:
"""Read-only COIN-D6 LiDAR -> sensor_msgs/LaserScan. Opens the port O_RDONLY; sends nothing."""
import fcntl
import math
import os
import select
import termios
import threading
import time
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.duration import Duration
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rosorin_base.coin_d6 import PacketParser, decode
BINS = 400 # 0.9 deg, matches the measured ~401 samples/ring
class LidarReader(Node):
def __init__(self):
super().__init__('rosorin_lidar_reader')
self.dev = self.declare_parameter('device', '/dev/lidar').value
self.frame_id = self.declare_parameter('frame_id', 'laser').value
# Native angles run clockwise: launch sets clockwise=True (floor test 2026-09-28, docs/hardware.md)
self.offset = self.declare_parameter('angle_offset_deg', 0.0).value
self.clockwise = self.declare_parameter('clockwise', False).value
self.range_min = self.declare_parameter('range_min', 0.12).value # masks self-returns (<=106 mm)
self.range_max = self.declare_parameter('range_max', 8.0).value
self.pub = self.create_publisher(LaserScan, 'scan', 10)
self.parser = PacketParser()
self.stale_s = self.declare_parameter('reopen_after_silence_s', 2.0).value
self.fd = self.open_port()
self.ring = None
self.ring_t = None
self.running = True
self.thread = threading.Thread(target=self.loop, daemon=True)
self.thread.start()
self.get_logger().info(f'reading {self.dev} (read-only)')
def open_port(self):
fd = os.open(self.dev, os.O_RDONLY | os.O_NOCTTY)
fcntl.ioctl(fd, termios.TIOCEXCL)
a = termios.tcgetattr(fd)
a[0] = a[1] = a[3] = 0
a[2] = termios.CS8 | termios.CREAD | termios.CLOCAL
a[4] = a[5] = termios.B230400
a[6][termios.VMIN] = 1
a[6][termios.VTIME] = 0
termios.tcsetattr(fd, termios.TCSANOW, a)
return fd
def reopen(self, why):
"""USB re-enumeration (seen 2026-09-28: EPIPE, ttyUSB1 -> ttyUSB0) leaves the old fd dead."""
self.get_logger().warn(f'{self.dev}: {why}; reopening')
try:
os.close(self.fd)
except OSError:
pass
self.fd, self.ring, self.parser = None, None, PacketParser()
while self.running:
try:
self.fd = self.open_port()
self.get_logger().info(f'{self.dev} reopened')
return
except OSError:
time.sleep(0.5)
def loop(self):
last_data = time.monotonic()
while self.running:
try:
ready, _, _ = select.select([self.fd], [], [], 0.2)
data = os.read(self.fd, 1024) if ready else None
except OSError as e:
self.reopen(f'read error {e}')
last_data = time.monotonic()
continue
if data == b'':
self.reopen('EOF (device gone)')
last_data = time.monotonic()
continue
if not data:
if time.monotonic() - last_data > self.stale_s:
self.reopen(f'no data for {self.stale_s:.0f} s')
last_data = time.monotonic()
continue
last_data = time.monotonic()
try:
for pkt in self.parser.feed(data):
start, pts = decode(pkt)
if start:
now = time.monotonic()
if self.ring:
self.publish(self.ring, now - self.ring_t)
self.ring, self.ring_t = [], now
elif self.ring is not None:
self.ring.extend(pts)
except Exception as e:
if not rclpy.ok():
return
# a dead reader thread would leave the node up and silent for good: exit, launch respawns the node
# (base.launch.py respawn=True); meanwhile the base driver's scan gate holds the wheels
self.get_logger().error(f'reader thread failed: {e!r}; exiting for a restart')
os._exit(1)
def publish(self, pts, dt):
ranges = [float('inf')] * BINS
inten = [0.0] * BINS
for a, mm, i in pts:
if mm == 0:
continue
ang = (-a if self.clockwise else a) + self.offset
b = int(round((ang % 360.0) / 360.0 * BINS)) % BINS
r = mm / 1000.0
if r < ranges[b]:
ranges[b] = r
inten[b] = float(i)
m = LaserScan()
# REP/LaserScan: stamp = first ray. Ring is published when the next ring starts, so the first
# ray was dt ago. (Stamping 'now' put every scan ~100 ms ahead of TF: AMCL/costmap dropped scans,
# Nav2 collision 2026-09-28.)
m.header.stamp = (self.get_clock().now() - Duration(seconds=dt)).to_msg()
m.header.frame_id = self.frame_id
m.angle_min = 0.0
m.angle_increment = 2 * math.pi / BINS
m.angle_max = m.angle_increment * (BINS - 1)
m.scan_time = dt
m.time_increment = dt / BINS
m.range_min = self.range_min
m.range_max = self.range_max
m.ranges = ranges
m.intensities = inten
self.pub.publish(m)
def stop(self):
self.running = False
self.thread.join(timeout=1.0)
if self.fd is not None:
os.close(self.fd)
def main():
rclpy.init()
node = LidarReader()
try:
rclpy.spin(node)
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
node.stop()
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
Register the executable in setup.py's console_scripts list (the repo's setup.py has it):
On the robot:
'lidar_reader = rosorin_base.lidar_reader:main',
With the base service still stopped (Step 1), build and start the node in one terminal. This is how the first live
run on 2026-09-26 started it: the installed executable, with default parameters (clockwise False, which does not
matter for a rate check; Step 7 adds the launch parameters):
On the robot:
source /opt/ros/humble/setup.bash
cd ~/ros2_ws && colcon build --packages-select rosorin_base 2>&1 | tail -1
source install/setup.bash
install/rosorin_base/lib/rosorin_base/lidar_reader
Create ~/learn/scan_probe.py (the probe used on 2026-09-26): it waits for one scan and prints what is in it.
On the robot:
import rclpy, math
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
rclpy.init(); n=Node('probe'); got=[]
n.create_subscription(LaserScan,'scan',lambda m: got.append(m),10)
import time; t=time.time()
while not got and time.time()-t<5: rclpy.spin_once(n,timeout_sec=0.2)
m=got[0]; r=[x for x in m.ranges if math.isfinite(x)]
print("frame",m.header.frame_id,"bins",len(m.ranges),"finite",len(r),"min %.3f max %.3f"%(min(r),max(r)),"scan_time %.3f"%m.scan_time)
print("below range_min (self-returns kept in data, RViz/Nav2 ignore):",sum(1 for x in r if x<m.range_min))
In a second terminal:
On the robot:
source /opt/ros/humble/setup.bash
timeout 8 ros2 topic hz /scan 2>&1 | grep -m1 "average rate"
python3 ~/learn/scan_probe.py
Check
Recorded on 2026-09-26, the first live run:average rate: 10.004 frame laser bins 400 finite 303 min 0.050 max 7.940 scan_time 0.100 below range_min (self-returns kept in data, RViz/Nav2 ignore): 5410 revolutions per second, 400 bins, frame
laser. The robot's own body is still in the data (54 values under
0.12 m);range_mintells every consumer to ignore them. That leaves about 249 of 400 bins with a usable return
(the "about 61 % valid" in the health numbers below). The first terminal logs
reading /dev/lidar (read-only). Stop the node with Ctrl-C.
If it fails
ros2 topic hzwaits forever: check the first terminal for an exception, andros2 topic list | grep scan. Right
after a node starts, another process may need a few seconds to see its topic (docs/lessons.md).- The node exits at once with an
OSErrorsaying "Device or resource busy" (EBUSY): the base service (or a
catfrom Step 1) still holds the port.- Your first test on the floor shows the room mirrored, or turning one way moves the scan the wrong way: the
clockwiseparameter is missing (Step 8).
In base.launch.py (chapter 11) the reader is one node. If you copied the complete launch file there, these lines
are already in it:
On the robot:
Node(package='rosorin_base', executable='lidar_reader', name='rosorin_lidar_reader',
respawn=True, respawn_delay=2.0, # a crashed reader comes back by itself (the scan gate holds the wheels)
parameters=[{'device': '/dev/lidar', 'frame_id': 'laser',
'clockwise': True}]), # native angles run CW: floor test 2026-09-28, docs/hardware.md
respawn=True is the other half of the os._exit(1) above: when the reader process dies for any reason, launch
starts it again 2 s later. Restart the base (and the mind after it, if installed):
On the robot:
sudo systemctl stop rosorin-mind 2>/dev/null
sudo systemctl restart rosorin-base
sleep 15
sudo systemctl start rosorin-mind 2>/dev/null
source /opt/ros/humble/setup.bash
timeout 8 ros2 topic hz /scan 2>&1 | tail -1
journalctl -u rosorin-base --since "-1 min" --no-pager | grep -i lidar | tail -3
Check
The last line ofros2 topic hz, as recorded on 2026-10-05 with everything running:min: 0.096s max: 0.106s std dev: 0.00160s window: 51One scan every 0.1 s, never more than 6 ms late. The status record after the reboot test on 2026-09-27 reads
"/scan 10.02 Hz". The journal showsreading /dev/lidar (read-only).
clockwise was foundThe factory configuration said the native angles ran counter-clockwise: the factory driver used them unchanged and
the factory launch did not flip them (docs/hardware.md, read-only on the factory partition, 2026-09-26). A
hand-held check that day was inconclusive because the owner's body was in the scan. The floor test on 2026-09-28
settled it: the robot turned in place at +0.5 rad/s for 3.2 s, and the script compared the scans before and after.
>> rotate +0.5 rad/s x 3.2 s
wheel x +0.000 y +0.000 yaw +88.9 deg
ekf x +0.000 y +0.000 yaw +82.5 deg
lidar angular shift: -81 deg (11 mm), -82 deg (11 mm), -80 deg (24 mm) [same sign as gyro = scan handedness correct]
The gyro measured +82.5 degrees (counter-clockwise, which the owner confirmed by eye); the scan shifted by -81
degrees. By the script's own rule the two must have the same sign, so the LiDAR's angles run clockwise. The fix is
the clockwise: True parameter; the robot's forward is still the LiDAR's 180 degrees, so the transform's yaw of pi
did not change. The factory-derived "left = native 270 degrees" was wrong: left is native 90 degrees. After the fix
a turn to the right read gyro -84.5 degrees, LiDAR -83 degrees. That test drives the wheels; chapter 16 runs it
(scripts/floor_odom_test.py) as part of the odometry work.
Both tests below were run on this robot with the wheels disabled. Nothing moves. Do them on the stand.
This unbinds the LiDAR's USB device for 3 s and binds it again, which is what a loose cable or a re-enumeration does:
On the robot:
echo 1-2.2.4 | sudo tee /sys/bus/usb/drivers/usb/unbind >/dev/null; sleep 3; echo 1-2.2.4 | sudo tee /sys/bus/usb/drivers/usb/bind >/dev/null; sleep 6
ls -l /dev/lidar
journalctl -u rosorin-base --since "-15s" --no-pager | grep lidar_reader | tail -4
Check
Recorded on 2026-09-28:lrwxrwxrwx 1 root root 7 Sep 28 05:54 /dev/lidar -> ttyUSB0 Sep 28 05:54:18 rosorin bash[23392]: [lidar_reader-3] [WARN] [1790574858.722578236] [rosorin_lidar_reader]: /dev/lidar: EOF (device gone); reopening Sep 28 05:54:22 rosorin bash[23392]: [lidar_reader-3] [INFO] [1790574862.240106030] [rosorin_lidar_reader]: /dev/lidar reopenedReopened 3.5 s after it noticed the loss;
/scanresumed without a restart.
scripts/tests/scan_gate_live.py checks that the driver sees the wheels are disabled, kills the reader with SIGKILL,
watches the driver's own scan_age and checks that a new reader process appears:
On your laptop:
ssh rosorin 'mkdir -p ~/setup/tests'
scp ~/CCode/rosorin-pro/scripts/tests/scan_gate_live.py rosorin:setup/tests/
On the robot:
python3 ~/setup/tests/scan_gate_live.py 2>&1 | tail -4
journalctl -u rosorin-base --since "-20 s" --no-pager | grep -iE "lidar|respawn|process has died" | tail -5
Check
Recorded on 2026-10-02:{"scan_age_before": 0.07, "killed_pids": ["7806"], "worst_scan_age_s": 2.97, "scans_back_after_s": 3.03, "reader_pids_now": ["8146"], "state_blocked": "disabled"} PASSand in the journal
process has died [pid 7806, exit code -9, ...], 2 s laterprocess started with pid [8146], thenreading /dev/lidar (read-only). Scans were back 3 s after the kill. With the wheels enabled and
driving on the stand at 0.35 m/s, killing the reader stopped the wheels 0.95 s later through the driver's scan gate
(the driver held them 0.52 s after the last scan), and they turned again by themselves 4.5 s later, with no reset
(driver log "wheels released";docs/motion_safety.md, 2026-10-02).
Why this matters
On 2026-09-30 a vision model on the robot used so much shared memory that the kernel killed robot processes and
the LiDAR reader stalled (docs/lessons.md). On 2026-10-02 a test on an isolated domain showed that with the
LiDAR silent, Humble's collision monitor passes velocity commands through unchanged, because it ignores a scan
older than its timeout. Every guard that depends on the LiDAR goes quiet together. The respawn and the driver's
scan gate (chapter 11) are the answer; "it stops when X fails" is tested by making X fail.
What a healthy LiDAR looks like on this robot, and who watches each number:
| Number | Healthy value | Source / watcher |
|---|---|---|
| Rate | 10 Hz; 0.096-0.106 s between scans | ros2 topic hz /scan; self-care (chapter 23) fails topic_rates below half the usual rate |
| Silence | a scan at least every 0.5 s | the base driver's scan gate holds software wheel commands after 0.5 s without a scan (chapter 11) |
| Explorer's check | >= 7 Hz over 2 s, >= 45 % valid returns (mean of 5 scans), newest scan < 1 s old | behavior/room_explorer.py scan_health (chapter 24) |
| Valid returns | about 61 % of the 400 bins | baseline 2026-10-01 (docs/decisions.md) |
| Field of view with returns | 227 degrees; the 133 degrees behind are the blind sector plus the arm tower's shadow | measured 2026-10-05 (nav.launch.py comment) |
| Rear blind sector | about 65 degrees, 138..180..155 degrees | docs/navigation.md; why the robot never reverses (chapter 18) |
| Range | 0.12-8.0 m published; longest return seen 7.94 m | lidar_reader.py, docs/status.md |
| Self-returns | native 33-57 degrees at 51-88 mm and 291-316 degrees at 50-106 mm | below range_min, ignored |
| Noise | median 6 mm per degree on a static scene over 28 revolutions | docs/hardware.md |
| CPU | about 12 % of one core | selfcare/expected.json |
The readback script of chapter 29 also checks that /dev/lidar exists.
ls -l /dev/lidar points at a ttyUSB device.python3 -m pytest -q test/test_coin_d6.py ends with 3 passed.ros2 topic hz /scan shows about 10 Hz with the base service running.scan_probe.py prints frame laser and 400 bins.reopening and reopened, and scan_gate_live.py prints PASS.rosorin-base (and rosorin-mind, if installed) are active again after the tests.Where this comes from
Repo (branchrebuild,bfb61d8):ros2/rosorin_base/rosorin_base/coin_d6.py,lidar_reader.py,
ros2/rosorin_base/test/test_coin_d6.py,test/coin_d6_sample.bin,ros2/rosorin_base/launch/base.launch.py
(lines 41-47),ros2/rosorin_base/launch/nav.launch.py(localizer comment),ros2/rosorin_base/setup.py,
scripts/install_ch341.sh(udev rule),scripts/tests/scan_gate_live.py,scripts/floor_odom_test.py,
behavior/room_explorer.py(scan_health),selfcare/expected.json,selfcare/checks.py. Docs:
docs/hardware.md(LiDAR protocol 2026-09-26, orientation and its correction 2026-09-28),docs/lessons.md
(2026-09-26 serial and protocol lessons; LiDAR USB re-enumeration 2026-09-28; 2026-09-30 OOM; 2026-10-02 silent
LiDAR),docs/navigation.md(2026-09-28 collision, rear blind sector),docs/status.md(LiDAR driver
2026-09-26, base bringup and reboot 2026-09-27),docs/decisions.md2026-09-26 (native angles + TF) and the
scan-health baseline,docs/motion_safety.md(scan gate stand test). Command log: raw read 2026-09-26 23:11,
reboot check 23:31, capture and layout/checksum/encoding tests 23:51-23:53, failing test and fix 23:54, first
live run 23:54; rotation floor test and clockwise fix 2026-09-28 05:44-05:45; unbind/bind 2026-09-28 05:54;
respawn test 2026-10-02 16:17;/scantiming 2026-10-05 23:39. The packet and checksum walk-through was computed
for this guide on 2026-10-07 fromcoin_d6_sample.binwith the repo'scoin_d6.py.