Originally written for and published on the Foxglove blog

Project Context

This post is part of a new series about building a ROS 2 stack for the SO-101, an open-source 5-DoF robot arm - much like I've been doing all year for LeKiwi, a holonomic robot that uses the same STS3215 motors from Feetech. The SO-101 arm, just like LeKiwi, is normally operated using LeRobot, Hugging Face's framework for embodied AI, but I'm using ROS 2 instead, on top of sts_hardware_interface, the same ros2_control hardware interface that already drives LeKiwi's base.

Before I continue, a clarification on the title: I know Foxglove is a visualization and observability platform, not a simulator, and I know the title raises an eyebrow. But alongside the hardware interface, it ended up doing a simulator's job for me - providing the crucial visual feedback that told me whether the software was working before there was any hardware to check it against.

Mock Mode

Since assembling/calibrating the physical arm took time, I started working on the software stack in parallel, focusing on the controller configuration and a teleop scheme. Thanks to sts_hardware_interface's mock mode and the SO-101 URDF, I could mock the entire arm in software and didn't have to wait for the real hardware to be ready. In practice, I simply set the mock mode flag in the hardware interface configuration to true, telling the hardware interface to fake the servos' feedback internally, while the rest of the ROS 2 stack has no idea it isn't talking to real motors.

This works because of how ros2_control is structured: everything above the hardware interface - controllers, trajectory planners, collision checks - only talks to a standard set of command and state interfaces. The hardware interface translates commands into whatever data the actual hardware needs and publishes the state interface values that it reads from the hardware. Swap the real hardware with a simulated one, and the other elements in the ros2_control structure cannot tell the difference.

When the mock mode is enabled, instead of writing to a real servo bus, sts_hardware_interface fakes both sides of the interface - not just the commands, but the state feedback too. And it doesn't just echo back whatever you send it; it behaves as a real servo would - the voltage drops under load, temperature climbs with activity, and the current draw follows the commanded effort. In both real and mock cases, the emergency stop service does what it says, but the real motors are affected by gravity, and the arm naturally droops down. In mock mode, the joint motors stay in the same position.

Aside from external forces like gravity and collisions with obstacles, mock mode is sufficient to design and test high-level behavior. This meant that I could start building the software stack on my robot against the simulated arm, without needing to plug anything in.

Teleop with Inverse Kinematics

The first real feature I wanted was a natural way to move the arm: point at where I want the end effector to go, and let it figure out the joint angles by itself, rather than me driving the joints individually.

To do this, I first built a small virtual target that I could move around in 3D space - nothing exotic, just a simple coordinate frame that I could manipulate using joystick controls, which the end effector would then try to track. I also added a target_visualizer_node that publishes a colored marker at the target's pose, so that I can visualize it from Foxglove's 3D panel.

To track the target, I then added teleop_ik_node, which runs inverse kinematics (IK) using Pinocchio to continuously solve for joint positions that get the end effector to its target pose. I won't go into the IK math here - the repo has all the code if you're curious - but the experience of testing it is what stuck with me: I would nudge the joystick, watch the marker move, and the arm would chase it. Genuinely fun to work with, even in mock mode.

https://www.youtube.com/watch?v=V7oLxa4nlhU

This IK-based teleop made controlling the arm much more intuitive, but I also wanted the ability to control the joint angles individually when needed, especially for debugging. For this, I used Joint State Publisher, a community-contributed Foxglove extension available in the extension registry. To prevent conflicts between the IK teleop commands and the manual joint commands, I implemented a switching mechanism in joint_state_switch_node that can toggle between the two commands using a single service call, which can be triggered from a joystick button (using joy_teleop), Foxglove's Service Call panel, or my custom Button extension. It is incredibly satisfying to move the individual joints using the sliders, then switch to the teleop mode and watch the IK solver take over and move the joints to optimal positions, while maintaining the same target.

https://www.youtube.com/watch?v=eh6mlPwLT38

Self-Collision Checking

While testing teleoperation, I realized that the arm could easily be commanded into positions that would cause it to collide with itself. In simulation, this isn't a big deal - the meshes simply pass through each other, but on a real robot, this would break something. So, the next step was to implement self-collision checking that would run on every joint command, regardless of its source.

I used FCL, which checks the arm's own geometry against itself using bounding volumes. It's deliberately lightweight compared to using MoveIt, which wasn’t required at this stage - and it checks the whole path to a target, not just the destination, since two individually safe configurations can still cause a collision on the trajectory between them. Rather than just rejecting a colliding command outright, CollisionResolver first tries to clamp the target back to the last safe point along that path, and only rejects it if that fails. This can be seen in the image below.

The Agent Sidebar

A few weeks ago, the Foxglove team provided me with a year-long enterprise account along with early access to some of their newest features - and I've been dying to talk about it ever since. One of those features is the Agent Sidebar, a built-in AI agent that answers questions grounded in whatever data is actually loaded in the layout.

I used it mostly while debugging mock mode. Asking questions in plain English worked, but took a while to get an answer back; asking the same thing with an @ topic mention - pointing it straight at dynamic_joint_states, for example - was noticeably faster. Since the original layout did not show the dynamic_joint_states, it opened a raw message panel to capture the data.

One thing I liked was expanding the tool-call trace to see what it was actually doing behind the scenes, rather than just trusting the answer. I tried this on LeKiwi too, and it correctly figured out that LeKiwi doesn't use the standard Twist message for velocity commands, but TwistStamped instead, and adjusted what it queried accordingly - a small thing, but exactly the kind of detail that's easy to get wrong if you're not actually looking at the data.

The Agent Sidebar can also be pointed at your own MCP-connected agent instead of the built-in one, which I haven't tried yet - that's going in a separate post where I get into the AI tools I actually use day-to-day.

What I liked most was using the agent sidebar to build layouts. I gave the agent a generic prompt - to improve my layout without removing any existing panels - and it provided a detailed layout with three tabs: one for monitoring, one for control, and one for diagnostics. It also worked cleanly with 3rd-party extensions, including the Button panel and the joint state publisher.

https://www.youtube.com/watch?v=LxpaWsmgMm0

Turning Mock Mode Off

For the simulated arm, I used ROS 2 Kilted running on Pixi on my MacBook, which also brought up the Foxglove Bridge, letting me connect using the Foxglove app on the same device. Joystick inputs came from my Steam Deck, talking to the ROS 2 graph over Zenoh.

Once the real arm was ready and calibrated, I simply replaced use_mock:=true with serial_port:=... while running the launch file, and I was good to go. Of course, while useful, the simulation can never fully replicate the arm's real-world dynamics, so I need to do some additional tuning. For a ROS 2 package tested on a mocked arm, performance on the real arm is not too bad.

https://www.youtube.com/watch?v=yQKSiRwwoDI

What's Next

During these couple of weeks, I learned just how valuable mock mode can be - if your hardware interface can fake its I/O convincingly, and you have a visualization platform like Foxglove that can render the result in real time, you don't need working hardware to build against. But now that I have the real arm up and running, I want to start exploring its capabilities. The first thing I want to implement is a record-and-replay feature - capturing a sequence of movements of the arm as an MCAP file, and then replaying it back on demand. I've already started working on this, and there's a rough version taking shape. Once it is done, it'll be a blog post in its own right.

Since I'm thinking of recording MCAP files, I want to naturally try out Foxglove 3.0.0's new features that they launched recently. I'm looking forward to tinkering with Comparison Mode to load several recordings onto one timeline and see how closely they line up and where they diverge. This would be useful for comparing mock mode with the real arm, or even for fine-tuning PID coefficients further.

Everything I talked about in this post - the mock mode internals, the IK solver, the collision resolver - is provided in so_arm_ros2 and sts_hardware_interface repositories. sts_hardware_interface is in a stable place, but so_arm_ros2 is a work in progress. If you find something worth fixing or building on, I'd love to hear about it.

Building the SO-101

I had only built SO-100s before, so I went into this one expecting a familiar afternoon of fighting 3D-printed parts to get them to fit. Instead, it was noticeably easier - about 15 minutes, start to finish.

This wasn't just because I was familiar with the assembly - the design itself is more refined. There are fewer printed parts, the tolerances are better, and the fit and finish feel more polished out of the box. The cabling is simpler too: no more squeezing wires between printed parts, since everything now routes through dedicated channels built into the structure.

I have one complaint though: on the SO-100, the shoulder pan joint sits on a spare servo horn acting as a bearing of sorts; on the SO-101, it's just bolted to the pan motor and cantilevers freely - a bit of stability traded for easier assembly.

Not that I'm complaining; the SO-101 is genuinely a breeze to build. Kudos to The Robot Studio for an elegant redesign.

Updating the URDF

The SO-101's gripper has spaces for two hex nuts on the fixed jaw to mount accessories such as a wrist camera, with CAD files included in the SO-ARM100 repository. A wrist camera is super essential for most real applications of this arm, so it's a shame it's missing from the default URDF. I decided to fix that.

Getting the camera and mount into the URDF early mattered because I'm already planning self-collision checking down the line - whatever checker I build later depends on the robot's geometry from the URDF, so it needs to know that the wrist mount and camera exist from day one.

The mount mesh itself is straight from the SO-ARM100 repository - I didn't design it, just wired it into so101.urdf.xacro as a chain of fixed-joint links (wrist_camera_mount_link → wrist_camera_body_link → wrist_camera_frame → wrist_camera_optical_frame), ending in a proper REP-103 optical frame. SO-ARM100 doesn't ship a camera mesh, so I found one matching the camera I'm actually using - an Innomaker U20CAM-1080P, a plain UVC webcam - and added it as its own link next to the mount.

Now, I understand that not everyone wants a wrist camera, so the whole chain sits behind a wrist_camera_urdf xacro argument, on by default. Turn it off and the mount and camera links just aren't there - the self-collision checker only ever sees geometry that's actually part of the arm.

If you want to use this URDF, I've generated updated URDF and MuJoCo XML files for the SO-101 with the wrist camera integrated, and opened a PR here on the SO-ARM100 repository. The configurable Xacro files with the wrist_camera_urdf gating live in the so_arm_description package in the so_arm_ros2 repository.

Custom Wrist Camera Node

The obvious choice for a UVC camera on ROS 2 is usb_cam or v4l2_camera, both common ROS 2 packages, and either would have worked on Linux. But I want to ensure that so_arm_ros2's ROS 2 stack runs equally on my Raspberry Pi and my MacBook via Pixi and RoboStack, and neither of those Linux-only packages builds for Pixi's osx-arm64 target. I didn't want the wrist camera to be the one feature that only worked on the Pi, so I wrote a small generic driver instead: opencv_camera_node, built on cv2.VideoCapture rather than V4L2 directly. It picks the right backend per platform - V4L2 on Linux, AVFoundation on macOS - and otherwise doesn't care which one it's running on.

This custom node's design follows LeRobot's own OpenCVCamera: a daemon thread owns the blocking cv2.VideoCapture.read() call, and the non-blocking ROS publish timer just reads whatever frame is ready. That way a slow camera or a dropped frame never holds up the rest of the ROS graph. Another change compared to the standard ROS camera stack: neither image_transport nor camera_info_manager has Python bindings in this ROS 2 install, so rather than linking the C++ libraries from Python, the node publishes raw Image messages on image_raw directly and parses CameraInfo straight out of a standard camera_calibration-format YAML file itself.

I tested it on my MacBook via Pixi, and it worked as expected. Still need to calibrate the camera and populate the real intrinsics YAML file, but the basic pipeline runs fine with a dummy calibration file for now.

SO-Lamp

I've been working with STS3215 motors long enough to have amassed a fair amount of them, so after finishing the SO-101, I built a second one - this time with a lamp where the gripper would go. I had a spare IKEA Strala lamp cord set lying around, and it seemed like a perfect fit.

I kept it simple: 4-dof, with the SO-101's wrist redesigned into a lamp mount. An afternoon of CAD and printing later, the mount was ready. I paired it with a spare E14 bulb and a shade from a 3D-printable lamp design I found online.

For V2, I wanted it hooked into my home automation, so I ordered a Tapo E14 smart bulb, which is a bit bigger than the original, so I needed a lamp shade. I used MakerLab's customizable vase generator to design one, cut the bottom out, and added threads to screw it straight onto the Strala cord set. I really liked how it turned out.

My original goal was simple: I wanted a functional, actuated night lamp that I could control through my home automation, without unnecessary complexity. I briefly considered making it 5-dof, decided against it (despite people on Twitter telling me otherwise), and deliberately kept cameras and microphones off it, keeping the design focused solely on lighting and leaving out any AI features.

However, around this time I started seeing a surge of DIY and commercial robot-arm lamps online (see this survey by @irvinxyz on Twitter), which got me rethinking my own build. I settled for the head design from the open-source lamp (image below) from Autonomous.ai (which includes a 5th motor) and decided to adopt it, keeping the rest of the SO-101's 4-dof structure intact.

After some adjustments, the new lamp head was ready: I added M3 heat-set inserts and scaled down the original design. Mechanically, that's done, but the electronics are still catching up. The plan is to drop the Raspberry Pi for a XIAO ESP32-S3 Sense - Seeed Studio's Sense module packs a mic, camera, and microSD slot, which covers most of what I wanted to add - but it hasn't arrived yet. I'll pair it with a XIAO-compatible Bus Servo Driver to power and control the STS3215 motors directly, which I'm also waiting on. For lighting, I'm swapping the E14 bulb for an off-the-shelf LED ring wired through the servo driver. Once it's all in, everything will live in the lamp head with just a single power cable running up to it. I love how the new head and 5-dof design look - the electronics and the 3D-printed cover are just going to take a bit longer.

I still needed an actual night lamp good enough for reading in bed, so I put the now-freed-up E14 cord set and smart bulb into a non-robotic 3D-printable lamp I found online. Definitely the most beautiful lamp I've ever 3D-printed.

Back to the SO-Lamp: my favorite decision in this whole build was keeping it based on the SO-101 design. No new URDF to generate, no extra controller config beyond the lamp head changes. This makes life easier whenever I get around to simulating or controlling it in ROS 2.

Microduck

While I was building the new lamp head, Pollen Robotics announced the Microduck - a tiny bipedal robot that's compact, versatile, and genuinely cute, for just $399. Between LeKiwi, the SO-101, and the SO-Lamp, I was pretty sure I didn't have time for another robot platform, so I almost skipped it.

A few things changed my mind. The Microduck runs on 15 Dynamixel XL330-M288-T motors, which alone cost 30+ EUR each - around 450 EUR just in motors - making $399 a steal before anything else is even counted. It also packs a camera, a ToF sensor (likely the VL53L5CX), a Rockchip RK3566 with an AI accelerator, two IMUs, a mic, and speakers - worth salvaging for parts alone if I ever get bored of the robot itself.

It's not just the hardware, either - it ships with a simulation platform, a sim-to-real pipeline, and Rust-based firmware, which makes it a solid excuse to tinker with both reinforcement learning and Rust. While waiting for mine to arrive, I couldn't resist getting a head start: I took the meshes and MuJoCo files Pollen Robotics provided and put together a URDF and Xacro file for it, in case I ever want to bring the Microduck into ROS 2 down the line (even though I promised myself I wouldn't). It's up on microduck_description if you want to poke around.

I'm glad I was sold on the Microduck and pre-ordered it quite fast - Pollen Robotics sold 10,000+ Microducks in the first few days, after which they dropped the pre-Christmas delivery promise and pushed the estimate out to 4-6 months. If all goes well, I should have my Microduck by the winter break. Fingers crossed.

What's next

So around the holidays I'll be stretched thin between my current robots and the new Microduck. There's another new robot arriving around the same time (still WIP; more details to follow), so time management will matter. My goal is to finish the SO-Lamp before the MicroDuck shows up, then hand the hardware off to someone else (and the open-source community) to take it further.

For the SO-101, the wrist camera's optical frame math is done, but not calibrated - I want to calibrate it just like in my earlier blog post. Beyond that, IK teleop and self-collision checking are the two things I keep talking about in this post, but that's its own story, coming soon.

Meanwhile, my UK visa has been approved, and I'm gearing up for my trip to ROSCon UK and the deep-dive session that I will be presenting there. I will be talking about the sts_hardware_interface and will use the LeKiwi and the SO-101 as examples.

Everything that I've talked about in this post lives in a new repository so_arm_ros2, which also includes a few things that I am yet to write about. As always, these projects are a work in progress and might change significantly over time. Still, feel free to poke around, provide feedback, or contribute if something catches your interest.

I finally got around to making the speaker work, but not in the way I expected. My original plan was to first test AEC and then jump straight to the fun part - wake word detection, speech-to-text, integration with AI agents such as Claude or nanobot, and finally text-to-speech. Instead, I found a much smaller and more immediately useful problem that I could fix with a speaker.

The problem

If you've read Under the Hood: LeKiwi's Joystick Controls, you'll know that every button-triggered feature on LeKiwi - e-stop, switching between teleop and Nav2, waypoint recording and following, saving the map - is a plain std_srvs/srv/SetBool service, callable from the Steam Deck, Foxglove (using either the Service Call panel or my Button extension), a CLI, or anything else that wants to. That's great for control, but it means there's no feedback: to know whether the e-stop actually engaged, or whether I'm in teleop or autonomous mode, I have to rely on how the robot is behaving.

With a working speaker now sitting on the top deck, the obvious first use for it wasn't a conversation - it was just having the robot tell me what it's doing.

With AKROS, my previous robot, I had LED strips that changed color based on robot state, and it was genuinely useful - one glance told me whether the e-stop was engaged without needing to guess or move the joystick to verify. LeKiwi never got the same treatment, mostly because there's nowhere left to attach LEDs. Every mounting point on the chassis is already spoken for, and I still need to fit a docking and charging solution onto it at some point, which only reduces the available space further. The speaker, on the other hand, is already installed from the reSpeaker work, so I didn't have to change any hardware. This made audio indicators the perfect solution given my constraints.

Decoupling the announcements

The naive way to do this would be to have every feature node call some kind of speak() function directly - twist_switch_node speaks when it flips, waypoint_recorder_node speaks when a waypoint is recorded, and so on. I didn't want that. It's the exact coupling problem I already solved once for control: if the announcement logic lives inside each feature node, every new feature needs its own Text-to-Speech (TTS) wiring, and the node has to know or care that a speaker exists at all. This would become quite messy.

So instead, I wrote indicator_node, a standalone node that doesn't touch any feature node at all. It watches ROS 2's built-in service introspection topics - <service>/_service_event - which every service already publishes regardless of who calls it. That means the announcement fires identically whether the request came from a joystick button, a Foxglove panel, or ros2 service call from a terminal, and adding a new announced service is a config change, not a code change in the feature itself.

Each _service_event stream carries a request event and a response event, so indicator_node matches the two up, remembers what was asked for, and only speaks once the response confirms whether the call actually succeeded. A failed call gets announced too, just with a different phrase - I'd rather hear "No waypoints found" than silence when I try to start a patrol I never recorded.

Centralizing the phrases

I didn't want spoken text scattered across the code either, so every phrase lives in one config file, phrases.yaml, keyed by service name, request value, and outcome:

services:
  /emergency_stop:
    "true":
      success: "Emergency stop enabled"
    "false":
      success: "Emergency stop disabled"

  /twist_switch:
    "true":
      success: "Autonomous mode"
    "false":
      success: "Tele-op mode"

  /record_waypoint:
    "true":
      success: "Waypoint recorded"
      failure: "Error"

A missing entry means "say nothing," which turned out to matter more than I expected. /record_waypoint, /reset_waypoints, and /save_map are all one-shot SetBool services where data: false does nothing - not a real state to announce - so I just left that entry out of the table entirely.

https://youtu.be/o_90obZyr4w

Pre-rendering with Kokoro

The other decision was when to actually synthesize the audio. Running a TTS model on every announcement, on a Raspberry Pi, in the middle of driving the robot (with computationally heavy features like Nav2, DepthAI), felt like the wrong place to spend CPU - especially since every phrase in phrases.yaml is small, closed-vocabulary, and known ahead of time.

So render_phrases.py (built with Claude's help) runs at build time instead, before setup.py collects data_files, and renders every phrase to a WAV file using Kokoro-82M via onnxruntime. It's incremental too - a phrase that already has a WAV gets skipped, and stale WAVs get pruned, so editing phrases.yaml only re-synthesizes the diff. The 116 MB Kokoro model itself isn't committed to the repo; it gets fetched from Kokoro's GitHub release only when there's actually something new to render, so a clean checkout with unchanged phrases never touches it at all.

What's next

Now that I have a working microphone array, a working speaker, and a working TTS implementation, I can move on to developing the AI-based voice assistant that connects to nanobot or Claude. But first, there are still some things that need to be done. I want to go back and finish what the reSpeaker post left unfinished - testing AEC properly, now that there's real audio coming out of LeKiwi's speaker while the mic is listening.

The second to-do item is to make sure that the indicator node doesn't play a phrase multiple times, as it does now. An example can be seen in the very first video in this post: In my example, the waypoint follower runs infinite patrols, which means that if I register a waypoint on a stationary robot and start the waypoint following, the audio indicator immediately replies with 'goal reached'. Since we have a single waypoint, the patrol loop starts again, and 'goal reached' plays again since the robot hasn't moved. This doesn't stop unless I kill the waypoint follower. This might not be the only issue, and I need to test this feature more extensively.

Beyond that, I honestly don't yet know how I'm going to wire nanobot (or whatever ends up being the AI-powered "brain") into the rest of the ROS 2 stack, let alone how that's supposed to coexist with indicator_node reaching for the same speaker to announce a service call mid-conversation. That's a real problem, and I don't have a good answer for it yet - so I'll leave it for another day.

indicator_node was meant to be a small, self-contained detour, but it ended up proving out the entire playback half of the voice assistant pipeline before I'd written a line of the conversational part. While I keep working on the indicator node investigations and investigating how to implement the pipeline, I've already started planning some hardware upgrades for LeKiwi - a battery monitor and a docking station. More on this when I've made some real progress.

The features described in this post are in the lekiwi_audio package. As always, this package, along with the rest of the LeKiwi ROS 2 stack, lives in lekiwi_ros2; it's a work in progress, so expect breaking or unstable changes once in a while. Meanwhile, feedback and contributions are welcome.

Originally written for and published on the Foxglove blog

This is the next chapter in my ongoing series of building a ROS 2-powered autonomous mobile robot based on the LeKiwi platform. Last time, I got a simple Nav2 configuration running end-to-end - mapping, localization, and point-to-point navigation, all visualized on Foxglove’s 3D panel - good enough to navigate to single targets, but it left a few things unfinished.

The most glaring issue: LeKiwi’s autonomous mode with Nav2 had collision protection, but the moment I switched to teleoperation, that safety net was gone. I could drive it straight into an obstacle, and the robot would happily comply. Alongside a fix for this issue, this post also covers two other features: waypoint following/patrolling so the robot can loop a route without the need to provide a fresh goal each time manually, and keepout / speed-limited zones painted onto the map so that the robot can stay away or slow down in areas where it might get stuck.

As always, the work covered in this post lives in the lekiwi_ros2 GitHub repository, which includes the full ROS 2 stack implementation for the LeKiwi platform.

Assisted Teleop and Collision Protection

My robot has two driving modes: autonomous navigation using Nav2, and manual teleoperation, and I can switch between them using twist_switch_node, a custom script that I wrote. In autonomous mode, Nav2’s collision_monitor watches the LiDAR scan and stops the robot before a collision. This is a really useful safety feature that unfortunately does not exist in teleop mode - if I drive the robot manually into a wall, it will happily keep going until it hits the wall. I wanted to fix that.

My original idea was to use Nav2’s built-in AssistedTeleop behavior - a behavior_server plugin that forward-simulates a teleop velocity and scales it down before a collision. It looked like a pure config change at first, since my joystick already publishes to /cmd_vel_teleop, matching AssistedTeleop’s expected input topic. Digging deeper, I realized that this does not work with my two-mode setup because AssistedTeleop is a behavior that runs only while Nav2 is active — it’s designed to let you steer the robot manually during an ongoing Nav2 session, not to protect standalone teleop.

I wanted to keep teleop and autonomous modes completely separate, so I had to think of a different approach. So I looked into collision_monitor instead - it structurally does the same job as AssistedTeleop, just with a zone-based check (FootprintApproach) rather than forward simulation. My original Nav2 configuration (the recommended method) places it upstream of twist_switch_node, so it only ever sees Nav2’s own commands. I moved it downstream instead, so it works irrespective of which mode - teleop or Nav2 - is currently active. This means the teleop mode is now protected by the same collision monitor as autonomous navigation, and the autonomous mode is unaffected. The wiring change looks like this:

This was much simpler than making AssistedTeleop work in teleop, and it doesn’t interfere with autonomous navigation at all - zero new nodes, just collision_monitor rewired. The only downside is that FootprintApproach’s zone check is a bit more conservative than AssistedTeleop’s forward-simulation, so the robot may stop a little earlier than necessary, which, for the LeKiwi, is not an issue at all. As you can see in the video below, it works as expected - the robot slows down as it approaches an obstacle, eventually stopping before hitting it.

https://www.youtube.com/watch?v=wtr3r-zxUMM

One issue came up during testing: I drove into a tight spot where collision_monitor stopped the robot, and then blocked every direction I tried on the joystick, leaving it completely stuck. To prevent this from happening again, I added a manual override: collision_toggle_node. This node sets collision_monitor’s FootprintApproach.enabled parameter to false when a preconfigured button is held down, and back to true when the button is released. It subscribes directly to /joy, with the button configured via a node parameter - R1 in my setup. Now, if I’m ever stuck like that again, I can just hold R1 to back away and release it to regain protection.

Waypoint Recording and Patrolling

In my previous Nav2 post, I was able to send the robot a single navigate_to_pose goal and watch it go there, but I had no way to make it work through a list of waypoints on its own. Each new target required a manual goal to be sent from Foxglove, which is tedious and not very autonomous. I wanted to be able to record a route of waypoints and have the robot patrol through them on its own.

Nav2 already ships nav2_waypoint_follower, a lifecycle node that drives the existing navigate_to_pose action once per waypoint in a loop - no new behavior tree required.

What it doesn’t ship is any convenient way to trigger it - the only interface is its FollowWaypoints action, which is callable only via an unwieldy ros2 action send_goal, with no direct way to trigger it from Foxglove. So I wrote waypoint_recorder_node, exposing three std_srvs/srv/SetBool services I can either bind to gamepad buttons via joy_teleop, or call from my Button extension on Foxglove. The three services are:

• /record_waypoint: captures the robot’s current pose as a waypoint, published as a visualization_msgs/msg/MarkerArray so the whole route shows up as arrows in Foxglove’s 3D panel

• /waypoint_follow: starts or resumes the patrol - a toggle, so a single button can alternate between starting/resuming and pausing the patrol

• /reset_waypoints: clears the route and all waypoints

In this new node, the waypoints are stored as a vector of geometry_msgs/msg/PoseStamped and sent to nav2_waypoint_follower via its FollowWaypoints action. Since that’s an ordered list with a FollowWaypoints.Goal.goal_index, the patrol can pause and resume mid-route instead of always restarting from the beginning.

https://www.youtube.com/watch?v=j2M1uxfEog0

Another feature I added is the ability to send manual navigate_to_pose goals while a patrol is running. In this case, the patrol treats it as a detour rather than a failure - go to the commanded target, then carry on following the original patrol route. Another feature where knowing the goal index is advantageous.

https://www.youtube.com/watch?v=IvBZlOj1wtg

The number_of_loops parameter controls how many times the patrol repeats - 0 (the default) loops forever, any positive number stops the patrol after exactly that many passes. I also implemented the ability to add new waypoints while a patrol is running - they queue up and join the route at the start of the next loop instead of splicing in immediately, which avoids preemption issues and keeps the robot from skipping waypoints.

https://www.youtube.com/watch?v=eoiM9-Zgxzg

I added two more features to make the patrol behavior more robust:

1. Error Handling: an unreachable waypoint gets retried a bounded number of times before being dropped from the route entirely, so the patrol keeps going instead of stalling forever.

2. Progress Reporting: the patrol’s current loop, current leg, ETA, and recovery count are published on /patrol_diagnostics as a diagnostic_msgs/msg/DiagnosticArray message, readable from Foxglove’s Diagnostics panel once configured to subscribe to /patrol_diagnostics, no custom message type needed.

This was a big addition - I can set up a patrol route, watch it run, and adjust it on the fly, all from Foxglove or a gamepad. That said, waypoint_recorder_node is nowhere near a polished solution - it’s a messy, partly vibe-coded proof-of-concept, and I plan to refine it iteratively. For now, if you want to use it yourself, proceed with caution and expect breaking changes.

Keepout and Speed-Limited Zones

This feature was born out of necessity. While testing my standard Nav2 config, I found a few areas in my apartment that the robot couldn’t navigate through safely. For example, my couch has a high clearance, so the LiDAR sees free space underneath it, but the pan-tilt mechanism gets stuck since it is taller than the clearance. Another example is my standing desk, which has a base that is lower than what the LiDAR sees, so the robot would collide if it tried to go under. I needed a way to tell Nav2 to avoid these areas.

While at it, I also decided to experiment with speed-limited zones, where the robot would slow down in certain areas. Not something I need right now, but a fun feature to play with. I wanted to be able to draw these zones on the map and have Nav2 respect them during navigation.

For both these features, I followed the handy tutorials in the Nav2 docs: Keepout Zones and Speed-Limited Zones. Both use a separate grayscale mask image layered on top of the navigation map, but they work differently:

1. KeepoutFilter marks any cell where the mask has a black pixel as a no-go zone in the costmap - the planner treats it as an obstacle and routes around it. I have keepout_filter running on both the global and local costmaps, so neither the planner nor the controller will enter those cells regardless of where the goal is.

   

2. SpeedFilter maps pixel brightness to a speed limit percentage instead - white means full speed, black means stop, and grays scale proportionally in between. In the image below, the two grey blobs are speed-limited zones, with the darker one indicating a lower speed limit than the lighter grey section. SpeedFilter only runs on the local costmap - it affects what the controller does in real time, but the global planner generates routes without accounting for it. This also means that if a keepout zone overlaps a speed-limited zone, the keepout zone takes precedence, since the global planner will never route a path through it.

   

The main challenge here was the recommended method for creating these masks: create a .pgm file in an image editor with the same size as the map, then overlay the map on top of it as a guide, and draw the zones. I updated the map_saver_node script to handle the first part: it now saves all-white .pgm placeholders sized to match the map alongside the rest of the map files, so there’s no need to create a correctly sized canvas from scratch.

However, to update these masks, I decided to look for a way to do it directly in VS Code, since I am already using it for development. I handed it off to Claude, and less than an hour later, I had a rudimentary PGM Editor extension that lets me open a blank .pgm file, draw shapes on it, and save it as a filter. In the image below, the black blob is a zone I’ve already drawn, and the red polygon is a new keepout zone being drawn. It is very basic, but it works for my needs. It is available on the VSCode Extensions Marketplace. Feel free to check it out and clone it to make it your own.

With the masks ready, I updated the Nav2 config and the launch file to include the costmap filter plugins and tested it out. Like before, using the Foxglove Bridge, I could visualize the costmaps remotely in Foxglove running on my laptop and see both the keepout and speed-limited zones in the 3D panel, as shown in the image below. Both masks are rendered as standard costmaps in Foxglove, so the colors are fixed, but you can switch to custom costmap layers if you want to pick your own.

Once the visualization was verified, I tested the robot’s behavior in the real world. The keepout zone worked as expected - the planner never routed the robot into the zone, always routing around it. The speed-limited zones also worked - the robot slowed down according to the speed limit set in the mask and returned to its original speed once it exited the zone. You can see both of these zones in action in the video below. I am super happy with how this feature turned out and relieved that it was so easy to implement.

https://www.youtube.com/watch?v=rKwyndPRBvg

Wrapping up

With these three features in place, the LeKiwi’s Nav2 setup is starting to feel complete. And like with everything else, Foxglove was one of the instrumental tools that really sped up my development process. From visualization to teleop and now the new navigation features, it works seamlessly whether I’m sitting at my laptop or holding the Steam Deck.

Beyond Nav2, the next major thing I want to tackle is a docking station: somewhere the robot can autonomously navigate to and charge. That breaks down into a few intermediate goals: tracking fiducial markers with the on-board OAK-D S2 camera, writing a custom docking behavior using Behavior Trees, and on the hardware side, designing the docking mechanism and finding a battery monitoring solution that can tell the robot when it’s running low. The hardware will probably take the longest, and might end up being a custom design if nothing off-the-shelf fits.

As for the planner and controller tuning that I promised in the previous post, it is still ongoing. I’ve been fine-tuning it iteratively as I use the robot, but it’s not the kind of thing that makes for an interesting blog post, so I will leave those changes in lekiwi_ros2 with the rest of the LeKiwi ROS 2 stack. As always, this is a work in progress, so expect regular updates and breaking changes at times. Feedback and contributions are always welcome!

I had no idea this would turn into anything more than a hobby. Since then, I've built several robot platforms (including AKROS, which became a major project), written more blog posts than I can count, given talks at local conferences, attended three ROSCons, and picked up a handful of freelance projects along the way.

Then ROSCon 2025 in Singapore happened, and changed my perspective entirely. I spent the week talking to freelancers and independent roboticists, and flew home with an idea I couldn't shake: make this side-hustle official. Also, build a robot, for which I chose LeKiwi mobile base and ended up buying a kit from Seeed Studio on my flight back.

Eventually, after some thought and some procrastination, I finally registered Kamath Robotics as an eenmanszaak — a Dutch sole proprietorship — with the Chamber of Commerce. That was exactly six months ago.

The Last Six Months

The original plan was simple on paper: split my time roughly 24 hours a week between freelance/contract work and my own projects that I hoped to eventually turn into products. In practice, a lot of that time went into LeKiwi — upgrading the hardware, building out its ROS 2 software stack, and writing my own packages along the way, like sts_hardware_interface, a hardware interface for STS-series servos (the STS3215 in particular) that powers platforms like the SO-101, LeKiwi, and XLeRobot.

The other development I didn't see coming was writing for Foxglove. In Singapore, Mat Sadowski — editor of the Weekly Robotics newsletter, now in developer relations at Foxglove — suggested I start writing for their blog. It was an easy yes: I was already building things, writing about them, and using Foxglove to visualize all of it, so the fit was obvious. Over the last six months, I've published several posts documenting the LeKiwi build while putting Foxglove's visualization capabilities through their paces. It's worked out well for both sides — Foxglove gets high-quality technical content grounded in a real build, and I get to keep doing the two things I like most: building robots and writing about them.

That same pattern repeated itself in May at the Open Hardware Summit in Berlin, where I got to talking with Seeed Studio's team at their booth, including Jinger Zeng from their developer relations team. I also got lucky as their ReBot arm demonstrator was stuck, and I was at the right place at the right time to flex my skills and get it working within minutes. This, along with some conversations with the team, ended up in an invitation to apply to their Seeed Rangers and Physical AI Ambassador programs. Once again, an easy yes — I was already deep into Seeed hardware: LeKiwi is built on their platform, I'd just ordered a reSpeaker Flex mic array after learning about it at the summit, and an SO-101 arm is next on my list to work with.

Speaking of conferences and summits, I can't forget this year's ROSCon in Toronto. Honestly, I couldn't be bothered - it is too far away and probably going to be ridiculously expensive, so I decided to be smart with my money and attend some local ROSCons instead. I sent in a proposal for a 10-minute talk at ROSCon UK on how I built sts_hardware_interface, and the organizers came back asking if I'd be okay doing a 45-minute deep-dive instead. And of course I was, 10 minutes was probably a little short to explain creating a hardware interface, but with 45 minutes, I can also do a demo. So ROSCon UK will be my first ROSCon as a speaker rather than just an attendee — my first ROSCon talk, period, and I'm pretty excited about it. Also, can't wait to be back in Edinburgh!

I'm also considering sending in a proposal to ROSCon Belgium about my work with LeKiwi and SO-101. Both talks lean heavily on Seeed Studio hardware, which fits neatly with representing them as an ambassador at both events; I'll effectively be repping both Kamath Robotics and Seeed at the same time.

Other than this, I've also been brewing a few things quietly on the side. One of these things is ros2-skill, an agent skill that lets an AI agent control ROS 2 robots directly through natural language, no rosbridge required. It partly draws inspiration from AgenticROS and ros-mcp-server, and builds on top of ros-skill. I've been testing it on my own LeKiwi project for a while now, quietly working out the rough edges.

As for the second thing, I can't really talk about it just yet — there's still some administrative work in progress on it. What I will say is that it involves a collaboration, and if it comes together the way I hope (fingers crossed), the Kamath Robotics fleet might be getting a humanoid soon. More on this once I'm actually able to share it.

Adjusting the Plan

My original goal of becoming a part-time freelancer and taking on contract work turned out to be harder than I expected. Running Kamath Robotics alongside a full-time job means everything has to happen outside my working hours, per my agreement with my employer. In practice, that's evenings and weekends. And this included freelance work, my personal projects, and blogging. This was getting a little too much.

That's not quite how it has played out, though. The Foxglove partnership, and more recently the one with Seeed, nudged me in a different direction entirely — toward written content, technical documentation, marketing, and product reviews rather than billable technical projects. I've also stopped actively looking for freelance or contract work on the technical side. I'm happy building my own projects and writing about them instead.

This shift has also changed who I reach out to. Rather than chasing client work, I've been talking to companies that build sensors, actuators, compute modules, and other robotics hardware and software, with the idea of incorporating their products into my own builds and then writing detailed articles or technical documentation about them.

A few years ago, a fellow roboticist suggested applying to conferences as "media" — it usually comes with a registration fee waiver and a bit more access than a regular attendee, in exchange for coverage. Back then, I was blogging mostly for myself, mostly as a build journal, and I had no company behind me to make that case. Now that Kamath Robotics is a real, registered business, I'm finally in a position to give it a try.

And that's exactly what I did for ROSCon UK. I shared information about this blog with the Tartan Robotics Collective (the organizers) and asked them whether they would consider me a media person. Turns out they did! I have already written about my previous ROSCon experiences, and I intended to write a blog about ROSCon UK anyway, only now I will be doing that professionally. This definitely validates the new direction I am taking Kamath Robotics in...

That extra breathing room has also opened up time to chase product ideas properly. LeKiwi keeps proving itself as I build on it, but it does come with its fair share of issues - the STS3215 motor variants are either too slow or too weak, and there's no variant that has the right balance, and they are all way too noisy. But while working on the platform, I've learned a lot, and I am ready to translate these learnings onto another robot platform that I will build from scratch. I've already got some ideas thanks to some interesting discussions online, and I've half-jokingly started calling this new robot the unofficial TurtleBot 5. More on this when the time is right...

To be clear (especially to potential clients), I haven't shut the door on freelance or contract work entirely. If something really interesting comes along and fits inside my existing hours, I'm still open to it. I'm just not actively chasing it anymore, unlike the media engagements I mentioned above.

The Next Six Months

Looking forward, I've got a few things on the list. First, is the SO-101 arm, I want to get it talking ROS 2 properly, rather than a thin interface with LeRobot, the same way I did with LeKiwi. At the same time, I want to design and implement features that let me control the arm with a Steam Deck and a VR controller, rather than needing a leader arm.

https://twitter.com/kamathsblog/status/2083151144310739091

Since I no longer need a second arm, I decided to use the motors for a different purpose. I assembled a second arm, stopped at the wrist flex joint, and attached a lamp to it instead. Less "pick and place", more a desk lamp with a mind of its own. I've got some ideas for this as well, more on this soon...

https://x.com/kamathsblog/status/2084967976457871722

Meanwhile, my search for a docking solution for LeKiwi continues. This is one of the last pieces to the LeKiwi hardware puzzle, now that the software is mostly sorted. I am also replacing the power-hungry Luxonis OAK-D S2 with a better depth camera that also draws lower current, and this should help with the automated docking. On the software side, I am also working on a few things related to voice control and AI that I will start posting about in the coming months.

https://x.com/seeedstudio/status/2085635572119523708

Finally, I want to take ros2-skill from "quietly testing it on my own robot" to something I'm actually comfortable pointing people at. That means cleaning it up, documenting it properly, and formally launching it — at which point I'll be actively looking for testers and contributors to help kick the tires on it. I hope I get this working in time to register a lightning talk at one of the upcoming ROSCons that I plan on attending...

Besides the projects themselves, I think Kamath Robotics is in a genuinely good place right now. Six months in, I'm spending my evenings and weekends on exactly what I set out to do — building robots, writing about them, and increasingly getting to do both for people who care about the same things I do. The partnerships and media access have also meant I can keep showing up to conferences and building the things I want to build, without it costing me a small fortune. Six months ago, this was a registration with the Chamber of Commerce and an idea; now it's a real, nearly sustainable way of working, and I'm proud of that.

Here's to the next six months. Onwards!

Fair warning: this one is going to be more technical than usual. It's a deep dive into some architectural decisions I made while building out LeKiwi's control layer - the reasoning behind them, the tradeoffs I considered, and the places where I deviated from standard ROS 2 patterns. If you're just here for a demo video or a step-by-step guide, you're out of luck. But if you want to understand why things are built the way they are, read on.

This is part of my ongoing series on building an autonomous mobile robot with the LeKiwi platform. The previous posts covered hardware upgrades, sensor calibration, and autonomous navigation. This one is about something more foundational to how I actually control the robot day-to-day: a service-based control architecture that works equally well from a Steam Deck or from Foxglove, and a wrapper node I had to write to make the joystick controls work the way I wanted.

Service-based Control Architecture

The most common method to control a ROS 2 robot with a joystick is to use teleop_twist_joy. It maps joystick axes to a Twist message and publishes it to /cmd_vel, with an optional deadman button. For simply moving the robot using a joystick, that's all you need. But LeKiwi doesn't use teleop_twist_joy, and the reason comes down to where the button logic lives.

With AKROS, I used teleop_twist_joy like everyone else. I also had other features on AKROS that were triggered by joystick inputs, which I handled with a custom node that subscribed to Joy messages, read the relevant button states, and triggered features like state change or motor halt directly.

That approach had a structural problem: it coupled every feature to the joystick. If a node subscribed to /joy to drive its own behavior, the only way to reach it is through a joystick, either physical or simulated. And it is difficult to scale either. If I added a new feature that needed a button press, I had to either touch the feature node itself or add a new node that subscribed to /joy.

It wasn't that straightforward to trigger these features from Foxglove either. I could have configured it to simulate my joystick mapping and produce fake /joy messages when digital buttons were pressed, but it was too much work at the time, and I also did not have AI tools at my disposal as I do now. The result was that the joystick features were only accessible from a physical joystick, which wasn't ideal for a robot that I wanted to be able to control from my laptop as well.

For LeKiwi, I went the other way: every non-teleop, button-based feature is a ROS 2 service. /emergency_stop (implemented as a first-class feature in sts_hardware_interface) , /twist_switch (implemented in the twist_switch_node in lekiwi_ros2), future waypoint-related features - all std_srvs/srv/SetBool services. Like I explained in my Nav2 post, /twist_switch is the node that selects which velocity stream reaches the motors: data: false routes joystick commands through, data: true hands control to Nav2.

The advantage here is that I now need a single node to simply read /joy and call the right service with the right payload. Additionally, I can trigger services from anywhere, so it does not depend on a physical joystick; the caller can be Foxglove, a CLI, or another node. The feature implementations themselves don't need to know or care how they are being called. Besides the single node, I would still need teleop_twist_joy to map the joystick axes to a Twist message, but that is not ideal.

This is where joy_teleop comes in. It's a ROS 2 node that subscribes to /joy and calls services or publishes topics based on a configuration file. It can handle multiple buttons, deadman buttons, and even axis mappings for Twist messages. The configuration is centralized, so adding a new feature or changing button assignments doesn't require touching the feature nodes themselves. The initial teleop.yaml config looked something like this:

joy_teleop:
  ros__parameters:
    teleop:
      type: topic
      interface_type: geometry_msgs/msg/TwistStamped
      topic_name: /cmd_vel_teleop
      deadman_buttons: [9]  # L1
      axis_mappings:
        twist-linear-x:
          axis: 1  # Left stick vertical
          scale: 0.2
          offset: 0.0
        twist-linear-y:
          axis: 0  # Left stick horizontal
          scale: 0.17
          offset: 0.0
        twist-angular-z:
          axis: 2  # Right stick horizontal
          scale: 0.68
          offset: 0.0
        header-frame_id:
          value: 'base_link'

    estop_press:
      type: service
      interface_type: std_srvs/srv/SetBool
      service_name: /emergency_stop
      service_request:
        data: true
      buttons: [0]  # A

    estop_release:
      type: service
      interface_type: std_srvs/srv/SetBool
      service_name: /emergency_stop
      service_request:
        data: false
      buttons: [1]  # B

    mode_nav2:
      type: service
      interface_type: std_srvs/srv/SetBool
      service_name: /twist_switch
      service_request:
        data: true
      buttons: [2]  # X

    mode_teleop:
      type: service
      interface_type: std_srvs/srv/SetBool
      service_name: /twist_switch
      service_request:
        data: false
      buttons: [3]  # Y

I refer to it as the 'initial' config because it has changed slightly since then, and I will explain more in the upcoming sections.

Foxglove and the Button extension

This is where the services approach pays off most visibly. As I've explained in the LeKiwi series, I am using the Raspberry Pi on the LeKiwi in headless mode and without any desktop environment. To control it from my laptop, I use Foxglove for both visualization and control. Foxglove has a Service Call panel that I can use to trigger any of the services from my laptop without needing a joystick. To make things even easier, I can use my custom Button extension (which is available on the Foxglove extension registry, directly from the app) to create a button for each service, so I can trigger topic publishers or service calls with a single click.

The service-based architecture also means all callers can reach the services using multiple methods: joy_teleop via button bindings, Foxglove's Service Call panel, my custom Button extension, a CLI, or another node - the robot doesn't care. The button assignments all live in one place, and adding a new caller never requires touching the feature node itself.

The Button extension is also designed to handle SetBool with a single button. In push mode, pressing calls the service with data: true and releasing calls it with data: false. In toggle mode, each click alternates between the two. Either way, one button is enough - the extension manages the value itself. Foxglove already didn't have this problem. I've written about this extension extensively in an earlier blog post, which is worth reading if you want to understand how it works.

Two buttons per action

As I kept adding features, the main limitation of joy_teleop became obvious: joy_teleop doesn't track state between button presses. Each service binding has a fixed payload configured in teleop.yaml, and every press sends exactly that payload - same value every time, no memory of what the last press did. Stateless by design.

This means that the SetBool mapping requires two buttons per service: one for data: true, one for data: false. Initially, this was fine - I had two SetBool services, /emergency_stop and /twist_switch, and four face buttons. A to engage the e-stop, B to release it, X to switch to Nav2 mode, Y to switch back to teleop. Four bindings, four buttons, perfect fit.

Then I added waypoint navigation. That meant at minimum: start the patrol, stop the patrol, record a waypoint, reset the waypoints - 4 buttons in total. I had only one button left - the Settings button, not even a face button. The option was to either give up on the new features or to redesign all of my feature nodes to be std_srvs/srv/Trigger services (which need a single input to toggle between states) instead of SetBool services. First of all, this meant a lot of work, both on refactoring the feature nodes and updating the Button extension to call Trigger services.

I didn't want to do that, because the idempotency of SetBool is actually useful for some features. For example, calling /emergency_stop with data: true multiple times doesn't make a difference; the motors stay stopped. If it were a stateful Trigger instead, a duplicate press from a jittery controller or a network retry could silently re-enable the motors at exactly the wrong moment. The predictable, stateless behavior is the whole point.

Also, I'm lazy. As I mentioned earlier, it's summer, the World Cup was on, and most importantly, I have a day job. I also didn't want to delegate this major refactoring to AI. So, I had to get creative.

The solution: bool_toggle_node

So I ended up writing bool_toggle_node — a thin wrapper that sits between joy_teleop and the SetBool services. Okay, I'll be honest, I had Claude write it for me, but I did the design and testing. It takes a list of SetBool services and exposes a corresponding list of Trigger services. When a Trigger fires, bool_toggle_node flips an internal boolean and calls the corresponding SetBool service with the new value — so joy_teleop gets one button per action, alternating between enable and disable on each press. The best part is that the feature nodes themselves are untouched, and the Button extension can still call the SetBool services directly without going through the bool_toggle_node layer, which is `joy_teleop '- specific.

The teleop.yaml config now includes a section for bool_toggle_node which looks like this:

bool_toggle_node:
  ros__parameters:
    toggles: ["emergency_stop", "twist_switch"]
    emergency_stop:
      trigger_service: /emergency_stop_toggle
      target_service: /emergency_stop
      initial_state: false
    twist_switch:
      trigger_service: /twist_switch_toggle
      target_service: /twist_switch
      initial_state: false

The rest of teleop.yaml describing the joy_teleop bindings are now updated to look like this:

    estop_toggle:
      type: service
      interface_type: std_srvs/srv/Trigger
      service_name: /emergency_stop_toggle
      buttons: [1]  # B

    twist_switch_toggle:
      type: service
      interface_type: std_srvs/srv/Trigger
      service_name: /twist_switch_toggle
      buttons: [2]  # X

    save_map:
      type: service
      interface_type: std_srvs/srv/SetBool
      service_name: /save_map
      service_request:
        data: true
      buttons: [4]  # Screenshot

So now, if I press B, joy_teleop calls /emergency_stop_toggle, which flips the internal state in bool_toggle_node and calls /emergency_stop, stopping the robot. Press B again, and the emergency stop is released. Same for X and /twist_switch, press once to enable autonomous mode, press X again for teleoperation. The remaining face buttons A and Y are now free for the waypoint features.

/save_map is a service implemented in the map_saver_node that saves the current map to disk. In my first Nav2 post, I subscribed to /joy to save the map, but I decided to change it to a service for consistency. The screenshot button on the Steam Deck, which now saves the map, is still a one-shot action and calls /save_map directly.

From Foxglove, nothing changes. The Button extension still calls /emergency_stop directly — a single button in push or toggle mode, the extension itself handles sending data: true and data: false. The bool_toggle_node Trigger layer is a joy_teleop-specific solution; Foxglove never needed it. The underlying SetBool services are untouched.

A couple of implementation details are worth mentioning. Each toggle's SetBool client uses a ReentrantCallbackGroup — rclpy's default MutuallyExclusiveCallbackGroup blocks the executor while a callback is running, so asynchronous calls inside a Trigger handler would wait forever for a response that can never arrive. The ReentrantCallbackGroup lets the executor re-enter and deliver the reply while the handler is suspended. The state flip only commits once the target service responds, so if /emergency_stop is unreachable, the toggle's belief doesn't drift.

One caveat: SetBool has no getter, so each toggle's state initializes from initial_state during launch rather than being queried from the actual service. If something calls a target service directly and bypasses the toggle node, the toggle's belief can drift. Not a problem in practice since I'm either using the Steam Deck (which goes through the toggle node) or the Foxglove Button extension (which calls the target service directly), and I'm not mixing the two. I intend to fix this in the future, but for now it's a known limitation.

What's next?

With this toggle infrastructure in place, I now have a few free buttons on the Steam Deck to implement new features - A, Y, and the screenshot button. I also have the R1 button free, which I've also used in an upcoming feature. I've actually already implemented these new features, but I will save the details for an upcoming post on Foxglove's blog. The new features are all related to waypoint navigation, and the post will cover how I implemented them, the challenges I faced, and how I overcame them.

The LeKiwi build is now nearly complete. I have plans to add a battery monitor and a docking station, but I'm still researching off-the-shelf solutions for both. I'm also facing an issue with my OAK-D S2 camera, which, despite being an amazing camera, draws way too much power from the Raspberry Pi, leading to crashes at run-time. I intend to replace it with a more power-efficient depth camera, and I have an option in mind, and I'm currently in talks to get a sponsored unit. More on this in a future post.

Alongside designing and researching solutions for the docking station, I want to move on to a few new projects. First, the SO-101 robot arm, which I've already assembled. The ROS 2 Physical AI Special Interest Group has a ROS 2 package for the SO-101 as well as a few ready-to-run examples on their GitHub, so that's my starting point.

Next, I've put together a small differential drive robot with an RPi Zero 2W, and I want to try vibe-coding the entire software stack for it — no writing or reading code myself. I just threw together some parts I had and have no real plans for it, but if I do something cool with it, I'll surely write a blog post.

Finally, as the LeKiwi build is coming to an end, I want to start adding some documentation to the GitHub repository, so that others can build their own and reproduce my results without having to go through my blog post ramblings. For now, the lekiwi_ros2 repo holds the full implementation, but be warned that it is still actively in development, so expect breaking changes. As always, feedback and contributions are welcome.

Originally written for and published on the Foxglove blog

Project Context

Over the past few months, I’ve been steadily upgrading my LeKiwi-based mobile robot platform to run autonomously with ROS 2. My earlier posts covered adding a LiDAR for 360-degree perception, calibrating a monocular camera for the base, teleoperating the robot using a Steam Deck, and finally configuring Nav2 to drive the robot autonomously.

This article is slightly tangential, but it focuses on the next major sensor upgrade: adding a stereo depth camera to give the robot 3D perception. The LeKiwi is originally designed to be a mobile manipulator, with space to mount a SO-101 robot arm, which I have always wanted to add. But while building the SO-101 arm, I realized that the first two joints are perfect for a pan-tilt mechanism, and decided to stop there. Then I remembered that I had a couple of Luxonis OAK depth cameras, and thought it would be fun to mount one on this pan-tilt mechanism to give the robot an active vision system that can look around and perceive depth.

This would not only be a fun hardware integration challenge and a learning experience, but it would also allow me to experiment with visual SLAM, visual-inertial odometry, VR integration, and dig a little deeper into vision-based AI. In this article, I will only focus on how I integrated the OAK-D S2 with ROS 2, and everything I did to visualize its data on Foxglove. If you are interested in the pan-tilt mechanism itself, I’ve covered that in a separate post on my blog.

Why the OAK-D S2?

The short answer is that I had one lying around.

But I had a choice to make. I have three Luxonis cameras, the original OAK-D from their very first Kickstarter, the OAK-D Lite, and a recently acquired OAK-D S2. The original OAK-D is a great camera, but it’s quite big and power-hungry, so I immediately ruled it out.

The real choice was between the OAK-D Lite and the OAK-D S2. Both cameras are pretty similar; they have an RGB camera, two monochrome cameras for stereo vision (with the same baseline), and the same Myriad X VPU. The S2 has a higher stereo resolution (800p vs 480p), better low-light performance, and a built-in IMU for visual-inertial odometry. The Lite is smaller and cheaper, but it doesn’t have the IMU and has lower stereo resolution. For my use case, the S2’s improved depth quality and VIO capabilities were worth the extra cost and size.

The real winning feature of the OAK-D S2 (in fact, any camera from Luxonis) is the DepthAI software stack, which provides onboard processing for stereo depth computation, VIO, and AI inference. This means I do not need to run heavy computation on my Raspberry Pi 5, which is already running the ROS 2 Control stack for motor control, LiDAR processing, and navigation.

These cameras can run custom pipelines that output RGB images, depth maps, visual-inertial odometry estimates, and even pointclouds, all with minimal CPU load on the host. This makes it an ideal choice for a mobile robot with limited onboard compute.

The Integration Journey

Luxonis maintains an official ROS 2 package called depthai-ros, which includes a modular driver that can be configured for different camera models and pipelines. I cloned it into my workspace and installed dependencies following the instructions in the repository. The package compiled cleanly, and the example launch files worked out of the box with RGB and depth images streaming immediately.

Configuration for Bandwidth Optimization

The depthai_ros_driver uses a pipeline-based architecture configured via YAML parameter files. The package includes several example configurations, but none combine RGB, depth, and VIO. So, I decided to create my own config tailored to my use-case: RGB images for AI inference, depth maps for obstacle avoidance, VIO as an additional odometry source, and optional pointclouds for mapping.

I created two custom configurations:

1. oakd_vio.yaml: Default config optimized for low bandwidth

2. oakd_vio_pcl.yaml: Adds RGBD pointcloud output to the bandwidth-optimized config (higher CPU load)

The bandwidth optimization was critical for two reasons: First, I’m running it on a Raspberry Pi 5 alongside other ROS 2 nodes, and some additional AI features I plan on adding. Second, I plan on streaming data to visualize on Foxglove, sometimes even remotely. So, I downscaled RGB to 1/4 ISP resolution with JPEG compression, reduced depth to 15 Hz with 2x decimation, disabled raw stereo image publishing, and kept VIO at 60 Hz for smooth camera pose tracking. I created the separate pointcloud config because I don’t need pointclouds during regular teleoperation or simple Nav2 operation. Keeping it separate reduces CPU load when not needed.

When pointcloud mode is enabled, the config changes significantly: depth rate increases from 15 Hz to 30 Hz to match the RGB rate, and the 2x decimation filter is disabled. 2x decimation would reduce the pointcloud to 4x fewer points, making it too sparse for useful visualization or mapping. Instead, bandwidth is managed by compressing the full-resolution pointcloud with Cloudini (more on this later), which preserves spatial detail while still achieving good compression.

Custom Launch File and URDF Integration

I started facing issues when I tried to integrate the OAK-D S2 URDF and set up the launch file. The depthai-ros package is designed to be a general-purpose package to support Luxonis’ entire product line of cameras, which means it provides a single parametrized URDF Xacro that can be configured for each camera model, and a comprehensive ~300 line launch file with all possible features and configuration options. This makes perfect sense for a general-purpose driver, but for my single-camera robot, this complexity was overkill.

I created a minimal 80-line launch file and a custom URDF macro tailored specifically for the OAK-D S2 on my robot. The launch file does the following:

1. Switches between oakd_vio.yaml and oakd_vio_pcl.yaml based on a pointcloud argument

2. Starts the camera driver as a composable node in a container

3. Overrides TF frame parameters to integrate with my existing robot (parent frame: tilt_link on the pan-tilt mechanism, base frame: oak_link)

The custom URDF was necessary because the automatic URDF generation in depthai-ros maps the OAK-D S2 to the OAK-D Pro mesh (the meshes are identical), but doesn’t include the IMU frame or offsets for the built-in IMU. My custom macro defines the camera base link (oak_link), IMU frame (oak_imu_frame), and uses the correct IMU offset from the OAK-D S2 datasheet.

Visualizing with Foxglove

Now that I had the camera configured and the launch file set up, I could test the integration and visualize the data in Foxglove. I launched the pan-tilt mechanism with the OAK-D S2 camera and the combined URDF. On a separate terminal, I launched the Foxglove bridge. On my laptop (on the same network as the robot), I opened Foxglove and connected to the bridge.

# Launch the LeKiwi robot with the pan-tilt mechanism and the OAK-D S2 camera
ros2 launch lekiwi_bringup lekiwi.launch.py

# Launch the pan-tilt mechanism with the OAK-D S2 camera
ros2 launch lekiwi_bringup lekiwi.launch.py config:=pantilt

# Launch the pan-tilt mechanism with the OAK-D S2 camera and pointcloud outputs
ros2 launch lekiwi_bringup lekiwi.launch.py config:=pantilt pointcloud:=true

# Launch the Foxglove bridge on a separate terminal
ros2 launch foxglove_bridge foxglove_bridge_launch.xml

3D Panel

To visualize the camera outputs, I relied completely on the 3D panel in the Foxglove app, using which I could visualize the following topics:

1. The robot’s URDF model with the pan-tilt mechanism and the camera mesh

   

2. The RGB image projected onto a frustum in 3D space

   

3. The depth map image, rendered as a pointcloud with depth-based coloring

   

4. The depth map image, with the RGB image used as a color map (by changing the RGB topic option on the left panel)

   

5. The pointcloud topic (when enabled using pointcloud:=true), rendered as a colored pointcloud

   

Pointcloud Compression using Cloudini

Despite the bandwidth optimizations, visualizing pointclouds remotely in Foxglove was still a problem. The 3D panel would freeze within minutes due to the sheer volume of data. So, I needed a better solution.

I turned to Cloudini, a pointcloud compression library by Davide Faconti that achieves impressive compression ratios using lossy quantization. Better yet, it includes a Foxglove extension that lets you visualize compressed pointclouds directly without decompressing them on the client.

The initial setup was simple: clone the repo, build it in my ROS 2 workspace, and add it to my launch file. The documentation was clear, and I had it running in about 10 minutes. But I faced some issues along the way.

Cloudini Foxglove Extension

To visualize the compressed pointcloud in Foxglove, I needed the Cloudini Converter extension from Foxglove’s extension registry. The extension appears in Foxglove’s available extensions list and can be installed directly from there.

After installing the extension, Foxglove was able to decode the compressed pointcloud topic, and I could select it from the Topics section of the 3D Panel options. That confirmed the compressed pointcloud data was now being decoded correctly, but it revealed a new issue: all the colors were gone.

The RGB Quantization Problem

While the uncompressed pointcloud had perfect RGB colors, the compressed pointcloud showed up completely black in the BGR color mode on Foxglove. I even had to change the app’s color scheme to light mode to visualize the pointcloud properly.

After digging into the Cloudini source code, I found the issue. The depthai-ros driver stores RGB data as a bit-packed integer inside a FLOAT32 field. Cloudini applies lossy quantization to all FLOAT32 fields by default, assuming they’re XYZ coordinates. This destroys the bit-packed color data.

The problem is in how the depthai-ros pipeline packages RGB data for pointclouds. Since I did not want to push AI-generated changes to the depthai-ros or Cloudini source code, I ended up writing a custom pointcloud compressor node that uses Cloudini as a library but applies field-specific quantization. The node loops through all pointcloud fields after Cloudini’s initial encoding setup, checks for color field names (rgb, rgba, bgr, bgra), and exempts them from quantization by clearing their resolution parameter. XYZ fields still get the 1mm quantization for compression, but color data stays intact. This does impact the compression ratio, but solves the visualization problem. I updated my launch file to load this custom node when pointcloud:=true.

This worked perfectly. The compressed pointcloud now has full RGB colors, the bandwidth is dramatically reduced, and the visualization is nearly identical to the uncompressed pointcloud. Most importantly, the visualization no longer freezes. There’s still some lag when I move the pan-tilt mechanism, as the compressed pointcloud takes a moment to update, but it is not too bad and quite usable for remote visualization.

Wrapping Up

This integration took longer than I expected, but I learned a lot about depth cameras, bandwidth optimization, and the quirks of pointcloud data formats. Foxglove was essential throughout; being able to visualize RGB, depth, TF frames, and pointclouds simultaneously made debugging so much faster.

Cloudini is an amazing library, and I’m grateful for the open-source work that Davide Faconti has put into it. While I’m not entirely sure how well compressed pointclouds will work for features like visual SLAM, it is a fantastic solution for remote visualization.

At this point, the OAK-D S2 is fully integrated. RGB streams at 30 FPS, depth at 15 Hz, VIO at 60 Hz. Compressed pointclouds can be visualized really well in Foxglove with full color preservation. While the pan-tilt mechanism and the camera are ready for visual SLAM and AI-based perception, that’s a project for another day. For now, the next step is to dig deeper into my LiDAR-based Nav2 implementation and implement some advanced features.

All the code related to the pan-tilt mechanism and the OAK-D S2 can be found in the pantilt_ros2 GitHub repository, and it has been integrated into lekiwi_ros2 as a git submodule, if you want to dig into the details. As always, this project is an active work in progress, so expect regular changes. Feel free to reach out if you’re working on similar camera integrations - I’m happy to share what worked (and what didn’t).

Easier said than done. The robot's base is already packed - three motors, a battery, the servo control board, a voltage regulator, and a webcam - and the top deck has the Pi, the LiDAR, the IMU, and the pan-tilt mechanism with an OAK-D S2 depth camera attached. Most of the mic-and-speaker combos on the market are single boards shaped like smart-speaker pucks, built to sit on their own rather than be tucked into the gaps of an already-full chassis. I looked at a handful of options, and none of them solved this problem.

It took a trip to Berlin to find one that met my requirements. At the Open Hardware Summit 2026, I spent some time at Seeed Studio's booth and came across their new reSpeaker Flex lineup. Out of everything I'd looked at, this was the first solution that was actually compact enough to fit onto the LeKiwi. Just to be clear, this is only one of Seeed's reSpeaker lineup; they also make the smart-speaker pucks I was talking about earlier.

This post covers the capabilities of the reSpeaker Flex, how I mounted it on LeKiwi, and a demo script that tests the microphone features.

Meet the reSpeaker Flex (XVF3800)

The reSpeaker Flex is Seeed's mic array system built around the XMOS XVF3800 DSP, sold as two separate boards - a mic array board, and a control board carrying the DSP, USB-C, and the speaker output, connected by an FPC cable. That split is exactly what allowed me to mount it on the LeKiwi: the mic array can sit wherever it needs a clear view, and the control board can go wherever there's room, instead of needing one spot for both.

The mic array board comes in two variants. The Circular-4 arranges its four microphones in a ring for 360° pickup. The Linear-4 instead puts the same four mics in a row for more directional capture. Both mic arrays pair with the same control board.

For LeKiwi, the Circular-4 was the obvious pick - a robot gets approached from any direction, so 360° pickup matters more than aiming at one side of a room. And since the Raspberry Pi already runs the rest of the stack, I skipped the XIAO option and wired the control board's USB-C straight into the Pi.

Mounting on LeKiwi

With Circular-4 picked, the next question was where on LeKiwi each board would actually go.

The mic array board is a ring, and it turns out that the ring fits perfectly around LeKiwi's LD19 LiDAR. That puts the microphones up high, away from the wheels and motors, with a (nearly) clear 360° view for picking up voices from any direction.

The control board went the other way - tucked between the base plates, alongside the Pi and the rest of the electronics, just behind where a webcam used to sit. One cable runs from the control board up to the mic ring around the LiDAR, and a single USB cable connects the control board to the Pi. I decided to remove the webcam, which not only freed up space but also a USB port on the Pi. Since I already have an OAK-D S2 camera on the pan-tilt mechanism, the lower-quality webcam was redundant.

For the speaker, I just grabbed a small, off-the-shelf 5W 8Ω driver from Amazon, which plugged right into the control board's JST-PH connector. It's mounted on the top deck, right next to the mic array. That said, I haven't configured audio playback yet - the speaker is connected but silent for now, which means anything that requires simultaneous playback and capture (like AEC) couldn't be tested in this round.

With the hardware in place, it was time to get it running and see what the microphone could actually do.

Getting It Running

Plugging it in was the easy part - one USB cable into the Pi, a udev rule so I don't need root to talk to it, and pyusb installed. From there, everything below is really just a parameter on the XVF3800 that I can read over the same USB connection.

I first plugged it into the Raspberry Pi via the USB-C cable. Next, to get the host to actually talk to the respeaker, I needed to install pyusb and set up a udev rule for the reSpeaker device.

python3 -m pip install pyusb
sudo apt install alsa-utils

pyusb is what the test script uses to send control commands over USB. alsa-utils provides arecord, which the script shells out to for the recordings in the far-field and noise-suppression tests.

By default, talking to a USB device's control interface needs root, which would mean the test script would need to be run with sudo every single time. A udev rule fixes that by opening up permissions for this specific device's vendor/product ID as soon as it's plugged in. I saved this as /etc/udev/rules.d/99-respeaker.rules:

SUBSYSTEM=="usb", ATTR{idVendor}=="2886", ATTR{idProduct}=="001e", MODE="0666", TAG+="uaccess"

I then reloaded udev and replugged the device for it to take effect:

sudo udevadm control --reload-rules
sudo udevadm trigger

To get an understanding of the microphone's capabilities, I wrote the test_mic.py script, which runs through DoA, far-field capture, and noise suppression in one go, saving audio clips along the way:

python3 test_mic.py

Here's what each of those tests looks like in practice.

Direction of Arrival and Voice Activity Detection

First up was the direction of arrival (DoA). The XVF3800 can estimate which way a voice is coming from - 0 to 359° around the mic array - along with a voice activity flag (VAD) for whether anyone's actually talking at any given instant. For a robot, that's the obvious starting point: before LeKiwi can turn to face whoever's speaking, it needs to know which way to turn.

python3 test_mic.py --tests doa --doa-duration 30 Just polls DoA and VAD for 30 seconds and prints the results. Walk around the mic array while talking, and the angle should update while the VAD flag indicates whether someone's speaking.

Impressions: I tested DoA and VAD by walking around the robot while counting, and it was able to track the DoA quite decently. The beamforming provides a general direction of the sound source quite well, but it wasn't perfect, the angle would jump around a bit, but it could easily be attributed to the distance from the microphone and different surfaces around the microphone which could reflect the sound. For the LeKiwi, I would say that the DoA is good enough to turn the robot towards the person speaking, and then use the camera to identify the person and track them. The VAD worked perfectly, detecting actual speech and ignoring background noises.

Far-Field Capture and Automatic Gain Control

Next was far-field capture - can it actually hear me from across the room, not just right up against it? The XVF3800 leans on beamforming for the spatial side, paired with automatic gain control (AGC) to even out the volume, so a whisper from a meter away and a normal voice from across the room come out in roughly the same range.

python3 test_mic.py --tests farfield records the same phrase at 0.5m, 2m, and 4m, prompting me for each one and logging the gain alongside DoA/VAD so the clips and the numbers line up.

Impressions: As per the prompts from the script, I counted the same numbers with the same volume at 0.5m, 2m, and 4m away from the robot. The VAD was a bit weird and did not work at times, but the AGC adjusted the gain accordingly - a promising sign that LeKiwi should be able to hear commands from across the room, though I haven't done a real side-by-side listen of the clips yet to confirm.

Noise Suppression

Last up was noise suppression. The LeKiwi, especially with the STS3215 motors, is quite a noisy robot, which must be able to detect voices while operating with all that noise. Luckily for us, the XVF3800 handles both the steady stuff (fan hum) and the sudden stuff (a door slamming) entirely on-chip, with no extra work for the Pi.

https://youtu.be/aiFu5fVe94Y

python3 test_mic.py --tests noise records the same phrase with background noise running, once with that processing switched off and once with it on - a direct A/B, plus a rough before/after noise-floor comparison.

Impressions: In both cases, I drove the robot around while standing at the same place and counting with the same volume. The test runs in two phases, once with the noise suppression off and once when turned on. Comparing the two, it determines the amount of noise that was suppressed, and it seems to be working well. It was also able to detect speech using VAD in both cases, and the DoA also varied as I moved the robot around and rotated it, although this wasn't very accurate.

What's Next

With the mic array performing well for my voice-control application, the next step is to configure audio playback so that the speaker is actually usable, which will allow me to validate features like Acoustic Echo Cancellation (AEC).

Once the audio playback is working fine, I want to set up a pipeline that uses wake word detection and records voice commands using the mic-array, converts the speech to text, passes this to nanobot, and converts nanobot's response to speech, which is played back from the speaker.

Of course, I need to fine-tune this plan further. For now, I'm glad to have assembled the reSpeaker flex onboard the LeKiwi and made sure that it is working. The test script I used can be found in this GitHub Gist. You can find more resources and example applications in Seeed's reSpeaker Wiki.

As always, feedback and contributions are welcome. If you have ideas or experience setting up voice-control or voice assistants, then please reach out! This is my first time doing anything with audio, and I have a lot of questions.

Originally written for and published on the Foxglove blog

Project Context

This is the next chapter in my ongoing series of building a ROS 2-powered autonomous mobile robot based on the LeKiwi platform. So far, I’ve integrated a LiDAR, calibrated the onboard camera, implemented teleoperation using a Steam Deck, and added a BNO055 IMU with sensor fusion for reliable odometry.

With the perception stack, teleoperation, and odometry in place, autonomous navigation was the obvious next step. This post covers how I got Nav2, a popular ROS 2 navigation library, running on LeKiwi, from the sensor prerequisites through to sending navigation goals in Foxglove.

The implementation lives in the lekiwi_navigation package in my lekiwi_ros2 repository.

Step 1: Setting Everything Up

For the basic Nav2 setup, you need a reliable odometry transform (odom → base_link) and a clean LiDAR scan. I could also have gone down the vision route and used the on-board cameras for visual-inertial odometry (VIO) or visual SLAM (vSLAM), but I chose to keep it simple for now.

Odometry and Sensor Fusion

The STS3215 motors used in the LeKiwi provide accurate state feedback (position and velocity), so the odometry from the omni_wheel_drive_controller is quite reliable. However, since I had already integrated the BNO055 IMU, I fused its measurements with the wheel odometry using the Extended Kalman Filter (EKF) node from robot_localization to get a more robust odometry estimate. This provides a stable odom → base_footprint transform that both Nav2 and slam_toolbox rely on. I’ve written a detailed article about the sensor fusion implementation earlier on this blog.

https://www.youtube.com/watch?v=PFRJnBID5ME

I’ve now got a working odometry source that runs at 50 Hz and publishes filtered/fused odometry messages on /odometry/filtered, along with the required odom → base_footprint transform (Note: on my robot, base_footprint is the root frame of my platform, not base_link).

LiDAR Filtering

When I covered the LiDAR integration previously, the raw scan provided a 360° scan, but since then, I’ve added a pan-tilt camera mechanism that sits right in front of the LiDAR. Without filtering, part of the scan would show up as a permanent obstacle during mapping.

To fix this, I used laser_filters. The scan_to_scan_filter_chain node subscribes to the raw LiDAR output (now remapped to /scan_raw), applies an angular range filter that masks out the sector occupied by the pan-tilt mechanism (nearly 70°), and republishes on /scan. Everything downstream subscribes to this filtered scan topic.

# scan_filter.yaml (excerpt)
scan_filter_chain:
  - name: angle_filter
    type: laser_filters/LaserScanAngularBoundsFilterInPlace
    params:
      lower_angle: -0.6 # rad
      upper_angle: 0.6 # rad

With the filter in place, the scan displayed in Foxglove’s 3D panel looks as expected: a clean scan around the robot, with the obstructed sector correctly filtered out.

Step 2: Creating the Map

With odometry and a clean scan in place, the next step was to build a map so that I could navigate in it.

Mapping using slam_toolbox

I used the recommended approach of using slam_toolbox in asynchronous mapping mode to build the map. This package implements Simultaneous Localization and Mapping (SLAM) using a pose graph approach and builds a map incrementally as the robot moves. The asynchronous mode lets it process scans as fast as possible without blocking the rest of the system, which matters on a Pi 5.

The main tuning I did was to reduce the scan processing rate (the LiDAR runs at 10 Hz, which is more than slam_toolbox needs for a slow-moving indoor robot), cap the laser range to something sensible for indoor use (8 meters in my case), and push down the map rasterization frequency since that step is expensive and the default is unnecessarily aggressive. Nothing fancy, just bringing the defaults in line with the hardware.

I then drove the robot around until the map looked complete in Foxglove’s 3D Panel, watching it grow and close loops in real time.

https://www.youtube.com/watch?v=CI1aBMa5S0s

Saving the Map

You could technically keep using mapping mode for navigation; slam_toolbox would localize and update the map simultaneously. The problem is that it starts from scratch every session, which is not ideal. So, in order to have a consistent map for navigation, I needed to save the map and use it with a dedicated localization method for autonomous navigation.

slam_toolbox lets you save maps using service calls, but this means that I need to interrupt my mapping session, switch to a terminal, and call the service every time I want to save a map. To make my life easier, I wrote a small map_saver_node script that listens to /joy messages and triggers the save when I press the R1 button on the controller. This way, I can save the map on the fly. When triggered, the node does two things in sequence:

1. Calls /slam_toolbox/save_map service, which writes map.pgm and map.yaml for use with AMCL and nav2_map_server.

2. Calls /slam_toolbox/serialize_map service, which writes map.posegraph and map.data, the binary format slam_toolbox needs for its own localization mode.

Step 3: Localize in the Map

With the map saved, the next question is where the robot is within it. Nav2 needs a map → odom transform to plan and execute paths, which is the job of the localization layer. My original plan was to use slam_toolbox’s built-in localization mode, which can do this, but I also wanted to try AMCL for comparison, since I’ve been using it since ROS 1 and am more familiar with it. Eventually, I decided to keep both and use a launch argument to switch between them.

Option A: Localization using slam_toolbox

slam_toolbox offers a localization mode that reuses the serialized pose graph from the mapping step. It loads the previously saved map.posegraph and map.data files and runs scan matching against the stored structure, so it can handle loop closures and continuously refine the map as the robot moves. The robot starts from the map origin or from a known position configured at launch; either way, once scan matching locks in, it stays solid.

https://www.youtube.com/watch?v=DYTZX7KawUA

The catch is that the robot’s starting position needs to be somewhere recognizable in the saved map. On my setup, I just start the robot from the same spot every session, and localization snaps in immediately. In the above video, while the localization is perfect, the map has changed; I’ve closed a door and added an obstacle. Despite this, the localization is never lost, and in addition, the map is eventually updated, which can then be saved again.

Option B: AMCL

AMCL (Adaptive Monte Carlo Localization) works from the static map.yaml loaded by nav2_map_server. It runs a particle filter and can converge from any starting position; you just need to give it an initial pose estimate. The map stays fixed (no loop closures, no incremental updates), but in return, you can start anywhere and re-initialize at any time.

https://www.youtube.com/watch?v=ZyANdSa4yMw

For AMCL, I provide the initial pose estimate directly from Foxglove. The 3D panel lets you publish a 2D pose estimate by clicking on the map, which publishes a geometry_msgs/PoseWithCovarianceStamped on /initialpose, exactly what AMCL listens to. In the video above, I provide an approximate position estimate that is not too far away from the robot’s actual position. However, as I move the robot around, AMCL improves the position estimate. This can be seen from the covariance ellipse, which shows the uncertainty and reduces in size as the localization improves. One downside of AMCL is that it does not update the map, even though some minor changes are clearly visible. Despite this, the localization is still quite precise. However, if the environment changes drastically, I would not recommend AMCL unless a new map is created first.

Switching between Modes

Now, I have three separate operating modes: slam_toolbox mapping, slam_toolbox localization, and AMCL. I implemented all three in a single slam.launch.py with a nav_mode launch argument for switching between modes, which is set in the top-level bringup launch file and passed down. Here’s how to use it:

# Mapping with slam_toolbox
ros2 launch lekiwi_navigation navigation.launch.py nav_mode:=slam

# Localization with slam_toolbox
ros2 launch lekiwi_navigation navigation.launch.py nav_mode:=slam map_name:=studio

# Localization with AMCL
ros2 launch lekiwi_navigation navigation.launch.py nav_mode:=amcl map_name:=studio

Note that the map_name argument specifies which map to load for localization - it corresponds to the name of the folder created by the map_saver_node script, which contains all the necessary files for both localization modes.

Step 4: Navigating the Map

Now that I have everything in place - reliable odometry, a clean scan, a map, and the ability to localize in it - the last step is configuring Nav2 itself.

Nav2 Configuration

I wanted to get the basic configuration up and running, so I started with navigation_launch.py from nav2_bringup as a reference, and then stripped down the example configuration to use only what I needed. I put this in a custom nav2.yaml.

Nav2 is not a monolithic stack; it’s a set of separate nodes orchestrated by bt_navigator. When a goal arrives, bt_navigator runs a Behavior Tree: it asks planner_server for a global path, hands it to controller_server to follow it, and calls behavior_server if the robot gets stuck. I used the default Behavior Tree config, which is all you need for a simple point-to-point navigation project.

Since LeKiwi is holonomic, I went with MPPI as the controller choice with motion_model: Omni. For the planner, I went with SMAC 2D: A* on the costmap grid with no kinematic constraints, which suits a holonomic robot well. One thing to watch: velocity limits need to match the hardware and be set consistently across all Nav2 components, or MPPI will plan trajectories that the controller will reject. Once again, my goal was to get a simple configuration working first, so I did not dig into the advanced configuration options. My full configuration can be found here.

https://www.youtube.com/watch?v=gJRcThqgZx8

To send navigation goals, I once again used Foxglove’s 3D panel, which has a built-in navigation goal publisher (I had to change the default topic name to /goal_pose). I click a point on the map, drag to set the heading, and the robot starts navigating. Watching the costmap update, the planned path appear, and the robot follow it in real time in the same panel is what makes Foxglove particularly satisfying for this kind of work.

https://www.youtube.com/watch?v=nb6_P3YfnfQ

In the video above, I also show how to run the navigation stack in mapping mode. While it is fun to play with and quite useful for mapping unknown spaces without teleoperation, I would not choose this method over navigating on a pre-made map. The map provides crucial information about the robot’s environment, and the process of creating it is computationally expensive, so it is best to create it once and reuse it.

Switching between Twist Messages

With Nav2 generating velocity commands alongside teleoperation, I needed a way to switch between the two, as both publish to the same base controller and would conflict.

So, I wrote a twist_switch_node that lives in lekiwi_control, subscribes to Twist messages from both teleop and Nav2, and forwards one to the base controller via a SetBool service (/twist_switch) that selects the active source. I then configured joy_teleop to call this service based on button presses on the controller, so I can toggle between the two Twist sources without switching to a terminal. In the video below, I was using my laptop to visualize and control the robot via Foxglove, so I decided to use my handy Button extension for switching. You can find this extension in Foxglove’s extension registry, and I’ve also written about it if you are interested in reading how I built it.

https://www.youtube.com/watch?v=_G4hcKK4TVI

This node made testing genuinely fast and intuitive. I could send a navigation goal while still in teleop, manually reposition the robot, and watch the planned path update in real time in Foxglove’s 3D panel. Switching to autonomous mode at any point would immediately kick off navigation along the latest path. I’ve demonstrated this in the video above. In the video, I also show how the navigation stack responds when I set a new goal while the robot is already moving towards another goal.

What's next?

Getting Nav2 running on the LeKiwi was a fun and rewarding process, and Foxglove was a huge help throughout. The ability to visualize the map, the robot’s position, and the planned paths, while providing initial pose estimates and navigation goals directly from the same interface, made the whole process much smoother. The fact that I could do all of this remotely from my laptop, without needing a ROS 2 installation, was a game-changer for the development experience.

Now that the navigation stack is working well enough to be useful, I can start navigating around the house while iteratively optimizing the Nav2 configuration. Next, I want to explore more advanced topics such as different planners and controllers, speed-limited and no-go zones, autonomous mapping/remapping, and eventually implement waypoint following for more complex missions.

The full implementation so far is available in the lekiwi_ros2 package on GitHub. As always, please proceed with caution, as the package is constantly evolving and may contain breaking changes. That said, feel free to check it out, replicate it on your own robots, and let me know if you have suggestions for improvements. Happy navigating!

Talks

I flew from Amsterdam to Berlin on the morning of May 23rd and reached the summit venue - the TU Berlin mathematics building - by 10:30 AM.

At registration, I picked up my badge and goodie bag. I'd seen Kat Scott's LinkedIn post beforehand and had some idea what to expect, but I was still pleasantly surprised. Coming from developer conferences and trade shows, I'm used to branded notebooks, pens, and stickers. Here, the goodie bag included microcontrollers, a DIY air quality monitor kit, an iFixit precision screwdriver set, a Tony's Chocolonely bar (best chocolate in the world, btw), a calendar, a book, and a tote bag. Actual useful stuff. The badge wasn't paper either - it was a PCB (designed by bleeptrack), bundled with some parts: a coin cell battery, battery holder, headers, LEDs, and wires.

I couldn't check into my hotel yet and was already tired of lugging my backpack around, so I dropped into the auditorium to rest, listen to some talks, and plan my next move. I ended up sitting through quite a few across the day, with breaks to visit the exhibition area and talk to people. The full schedule is on the OHS26 website, and all recordings are on the OSHWA YouTube channel. Here are the talks that stood out to me:

Wearable Projectors for the Opera

This talk was already in progress when I entered the auditorium, so I only caught the second half. The presenters built wearable projectors for an opera performance at The Met Cloisters - a museum where cable runs weren't possible. It is always fun to see open hardware show up in the arts. They mentioned having the projectors with them at the event, and I wish I'd tracked them down - I'd have loved to see them working in person.

https://www.youtube.com/watch?v=m8o45XHPeHk

Building a DIY Scanning Tunneling Microscope

Given my background in high-precision mechatronics, this one caught my attention. Precision microscopes require both coarse and fine positioning stages, typically through two separate actuators. Sam Harley found a way to combine both into a single actuator - genuinely impressive engineering - and then open-sourced the whole thing. Definitely on my list of topics to dig into when I get the chance.

https://www.youtube.com/watch?v=Xmqj9LZ6IAg

AirGradient's Open Source Hardware Advantage

Achim Haug, founder and CEO of AirGradient, talked about building an open-hardware company that has shipped 50,000 air quality monitors to 80+ countries, and what open-source actually makes possible along the way - community integrations, open schematics, reduced lock-in. He also touched on AI-assisted development, including vibe-coding Tetris onto their product's display. A lot of this was directly relevant to where I'm thinking about taking Kamath Robotics, more on that later. As a sponsor, AirGradient also included a partial DIY kit of their indoor air quality monitor in the goodie bag, and I'm looking forward to putting it together.

https://www.youtube.com/watch?v=DNvcKuE2FbE

A Practical Guide to EU Regulations and Directives

I kept spotting Clemens Mayer around the event, thinking he looked familiar, and eventually placed him as a host on Element14Presents, a YouTube channel I frequently watch. His talk is essential viewing for anyone building and selling hardware in Europe. Key takeaways:

• CE compliance for open-source hardware? It depends on the product

• General Product Safety Regulation (GPSR) applies to almost everything

• EU Directives are adopted into national law; EU Regulations are directly applicable EU-wide

Watch the full talk for the complete list of directives and regulations worth knowing about.

https://www.youtube.com/watch?v=kk2fLntkyAU

The State of Open Hardware

Mitch Altman presented an honest look at the slow erosion of openness among the big names in the space. Arduino's acquisition by Qualcomm effectively ended its open-source stance. I knew Prusa had been drifting away from open-source, but wasn't aware of the specific licensing changes until this talk - changes that technically mean they're no longer open-source despite still using the label. Mitch also touched upon what happens when you bring AI into the mix. Worth watching if you care about where the ecosystem is heading.

https://www.youtube.com/watch?v=ZeEtrrLqNSM

Exhibits

The exhibition area, was distributed into four rooms, with some really creative projects being showcased. They also had empty tables where people could bring and share their projects. Falko Richter made a really nice overview video of the exhibition area.

https://www.youtube.com/watch?v=9H0Zhv0ZLKw

While there were many cool projects, here are some that caught my eye.

OpenScan

OpenScan is an open‑source 3D scanner that uses two NEMA stepper motors and a Raspberry Pi camera, with the firmware running on a Raspberry Pi and the chassis made from 3D‑printed parts. They even brought a giant Benchy printed from a 3D scan to demonstrate its scanning performance; you can see it (and the scanner itself) in Falko’s video above; unfortunately, I forgot to take pictures.

They were kind enough to give me a PCB kit to build my own, and I'm eagerly waiting for a free weekend to put it together. I’ve already soldered the PCB kit and started 3D‑printing the necessary parts.

Open Vacuum

A company (or group?) called Caznic showcased their open hardware vacuum cleaner built using existing vacuum cleaner components, a custom control board, and 3D printed parts.

For me, there were two standout design choices: first, the control PCB can be physically snapped into pieces and reconnected with wires for flexible mounting, and second, it runs on a power tool battery. We had a good chat about making it autonomous on a robot - something I might try. They also presented a talk at the summit.

Open Spider

This creator showcased a bunch of legged robots, and it was a lot of fun watching them run about. The tiny ones used small servo motors, a XIAO ESP32 S3 Sense board, and a battery. The mechanical parts were made from a single PCB that you could simply snap apart. I really liked the one with the furry cover, but it was being charged, so I couldn't see it walking around. There was also a larger one on display, as you can see below.

I totally forgot to get this creator's name or the name of this project, so I don't have links to it. If you are reading this and know more about this project, do let me know.

Open Hardware Boat

Another project I do not know much about is this boat with brushless motors for motion, a 5G/LTE HAT (I think) for communications, GPS, and a Unitree 4D LiDAR. It also had magnets on the side, which I think are for docking.

Nobody was at the stand when I was around, but they had a couple of diagrams showing their architecture, which included micro-ROS, ROS 2, Pixhawk, and Foxglove for visualization. I wish I could ask them more questions in person.

Seeed Studio

It was really nice to see Seeed Studio at the event. I've been buying their products for years, and I'm especially a big fan of their XIAO series of microcontrollers. While they had a lot of them at their booth, and they even gave one to attendees in their goodie bags, a few other things stood out for me:

1. reSpeaker: a series of microphone-based products for all sorts of applications, including AI and robotics. I really liked their circular microphone array that seemed perfect for my LeKiwi build. I've got an AI agent running on it, and I've been thinking about interacting with it using my voice. I also learnt that their linear microphone array is the same one that is used in the Reachy Mini, and turns out Seeed manufactures some other components for it.

1. Smart Glasses Project: A collab with an independent creator whose name I didn't quite catch. I really liked the idea of replacing the stem of an off-the-shelf pair of glasses with a PCB that includes a XIAO board, a battery, and a vibration motor. And voila, you have a pair of smart glasses. Not Meta Ray-Ban territory, but a solid DIY starting point.

1. reBot Arm: A 6+1 DoF, ~4.5 kg robot arm with a 1.5 kg payload, paired with the StarAI Leader Arm for teleoperation. I had a nice conversation with Jinger Zeng from Seeed's Developer Relations about it, and other robotics products from Seeed, like the LeKiwi (yes, my project is built on top of a Seeed product). When she realized I was a robotics engineer, she asked if I could fix their stuck demo, and even though I had never worked with the reBot arm before, I wasn't going to say no. I expected to dig through LeRobot scripts; turns out Seeed has their own webserver-based GUI (I unfortunately couldn't find more information about this on their website) for setup, also compatible with SO100/SO101 arms. The demo was back up in minutes, all thanks to the GUI. I'll be trying this with my own SO100 when I start working on it.

https://x.com/kamathsblog/status/2058621048694755336

Badge Hacking

Available on both days, the badge hacking area had a community-contributed box of components and everything you needed to get started. I began by soldering the coin cell battery and switch, which powered the badge, including the SAO (Shitty Add-On) connectors - my first encounter with SAOs, custom add-ons that people build and trade at such events. Several attendees had entire backpacks covered in them. Unfortunately, I didn't get pictures, but some were really creative.

I made a couple of mistakes: installed the SAO connectors backwards, and forgot to add resistors to the LEDs, so they now light up with uneven brightness. I'll fix them when I get the chance. I also added the XIAO from the goodie bag and soldered it directly without headers (like a psychopath); I have way too many of them at home, so I can afford to fix one to this badge permanently.

This turned out to be the best place to network - helping people with builds, picking up tips, and making new friends. The common consensus on the soldering tools was that Pinecil or similar products were far superior to what was provided, and I agree. The iFixit flush cutters, on the other hand, were superb, and I ordered a set on my way back.

Workshops

There were some really cool workshops on Day 2, but I had already made plans for the day, so I didn't register for many. I had free time in the afternoon, so I decided to join the KiCAD workshop by cpresser. It is a shame that all the workshops that I wanted to attend were during the same time slot, so I picked one that will actually be useful to me.

I had attempted to learn KiCAD a few times before, but had given up since the interface isn't beginner-friendly, especially when you're learning on your own. This one-hour session walked through the full PCB design workflow from schematic to routing, and suddenly, everything clicked. I'm excited to see what I can actually design now.

During the workshop, one attendee followed the entire exercise on a Steam Deck. I already use mine for programming, controlling, simulating, and visualizing robots. Turns out KiCAD installs natively without Distrobox workarounds, same for FreeCAD and OrcaSlicer (although I haven't tried those yet). The Steam Deck might genuinely be the perfect all-in-one device for end-to-end robotics development.

https://x.com/kamathsblog/status/2059015501385236758

Turning 33

Day 2 was my 33rd birthday. My only commitment was a KiCAD workshop, so I spent the morning at the Berliner Dom, grabbed souvenirs at the Humboldt Forum, and stopped for a smoothie in the courtyard—then realized I hadn’t installed KiCAD. I downloaded it over the free city Wi‑Fi and, while waiting, connected to my LeKiwi in Eindhoven; within a minute, I was visualizing the robot and driving its motors with almost no lag, hundreds of kilometres away.

For lunch, I had the best döner of my life - Dutch places really need to up their game, but maybe that’s for the best, or I’d eat it every day. Exhausted and overheated, I headed back to TU Berlin for a few unconference talks before the workshop. On the way, I stopped by an antiques market and bought myself a vintage beer stein and a model digger as birthday presents. Although I don’t drink anymore, the stein is already serving as a water glass for summer.

Wrapping up

This was my first hardware-focused event, and it felt nothing like developer conferences like ROSCon. Instead of attendees trying to talk about work or trying to sell you their product, people genuinely wanted to help, share, and learn - probably because most attendees are builders first, not rather than founders or employees. I caught up with Kimberly McGuire, a friend from ROSCon and fellow independent roboticist, and we talked about all things robotics. I also met other solo entrepreneurs, engineers, scientists, and artists - people from all sorts of different backgrounds; the mix of electronics with robots, music, textiles, health, and art made for genuinely interesting conversations.

As for project ideas, after seeing custom badges everywhere, I now want to make a badge for myself that I can wear to such conferences. I also want to build the AirGradient and OpenScan kits when I get the chance. I've ordered the reSpeaker kit from Seeed Studio, and I'm currently designing parts to attach it to the LeKiwi while it is in transit. To make space, I've removed the webcam from the LeKiwi base; having two cameras was a little too much anyway. The OAK-D S2 on the pan-tilt mechanism is now the only camera on the robot.

Several talks hit close to home: AirGradient's scaling story, Clemens's EU certification guide, and the unconference session on starting a hardware company. I've been considering moving away from consulting and freelancing, focusing instead on hardware product development, sustained by technical content creation. The talks and the conversations with other attendees validated that this pivot isn't a bad idea, even if it won't be easy.

I'm definitely looking forward to more hardware events now. I have the Eindhoven Maker Days to look forward to in September this year, and I'm already making plans for the Rome Maker Faire in October. I missed the Hackaday Europe event this year, but it is on my radar for next year. I don't know where the next OHS will be, but if it is in Europe, I'll definitely be there.

The wheel odometry works well (especially on short runs), thanks to the STS3215 servo motors providing accurate and reliable state feedback. But omnidirectional robots have a fundamental problem with dead reckoning: wheels slip sideways, and you can physically move the base without any encoder movement. Once the heading drifts, the dead-reckoned position estimate diverges along with it. You need something to anchor the heading.

And that something, in most cases, is an IMU. I recently added a BNO055 IMU and vibe-coded a ros2_control hardware interface for it using the bno055_hardware_interface package. Now it is time to integrate it into the LeKiwi stack and fuse measurements with wheel odometry using an Extended Kalman Filter from robot_localization. Later in this post, I also compare robot_localization with the newer fusioncore project.

Integrating the BNO055 IMU

The BNO055 is a 9-DOF absolute orientation sensor that does on-chip sensor fusion. It combines an accelerometer, gyroscope, and magnetometer, and outputs a quaternion in NDOF (Nine Degrees of Freedom) mode. The output is gravity-referenced and utilizes the magnetometer to correct drift, ensuring the orientation remains accurate over time without accumulating errors that can occur with raw gyro integration. This post focuses on the sensor fusion side and assumes you're already familiar with the hardware interface implementation covered in the earlier post, at least for this section.

The ros2_control Hardware Interface

I added the <ros2_control> block to the URDF in lekiwi_description, declaring the BNO055 as a sensor type — read-only, with no command interfaces:

<ros2_control name="lekiwi_imu" type="sensor">
  <hardware>
    <plugin>bno055_hardware_interface/BNO055HardwareInterface</plugin>
    <param name="i2c_bus">1</param>
    <param name="i2c_addr">28</param>
    <param name="axis_remap">P1</param>
    <param name="sensor_mode">NDOF</param>
    <param name="enable_mock_mode">false</param>
  </hardware>
  <sensor name="bno055">
    <!-- Orientation quaternion -->
    <state_interface name="orientation.x"/>
    <state_interface name="orientation.y"/>
    <state_interface name="orientation.z"/>
    <state_interface name="orientation.w"/>
    <!-- Angular velocity -->
    <state_interface name="angular_velocity.x"/>
    <state_interface name="angular_velocity.y"/>
    <state_interface name="angular_velocity.z"/>
    <!-- Linear acceleration -->
    <state_interface name="linear_acceleration.x"/>
    <state_interface name="linear_acceleration.y"/>
    <state_interface name="linear_acceleration.z"/>
  </sensor>
</ros2_control>

The plugin exposes ten state interfaces: a quaternion for orientation, three axes of angular velocity, and three axes of linear acceleration. Once the controller manager loads this hardware interface, these state interfaces become available for broadcasting.

Broadcasting to ROS 2 topics

To make the IMU data available to the rest of the stack, I configured the imu_sensor_broadcaster in lekiwi_control. This controller reads the state interfaces and publishes a sensor_msgs/Imu message to /imu_sensor_broadcaster/imu.

I actually discovered the imu_sensor_broadcaster while working on the omniwheel controller for the LeKiwi, and that's what motivated me to create the BNO055 hardware interface in the first place - I simply wanted to try it out.

imu_sensor_broadcaster:
  ros__parameters:
    sensor_name: bno055  # Must match URDF <sensor name="...">
    frame_id: imu_frame
    
    # ±2.5° magnetometer heading accuracy → 0.002 rad²
    static_covariance_orientation: [0.002, 0.0, 0.0,
                                    0.0, 0.002, 0.0,
                                    0.0, 0.0, 0.002]
    
    # Gyro noise density at 100 Hz → 1e-5 (rad/s)²
    static_covariance_angular_velocity: [1.0e-5, 0.0, 0.0,
                                         0.0, 1.0e-5, 0.0,
                                         0.0, 0.0, 1.0e-5]
    
    # Accelerometer noise
    static_covariance_linear_acceleration: [2.0e-4, 0.0, 0.0,
                                            0.0, 2.0e-4, 0.0,
                                            0.0, 0.0, 2.0e-4]

The covariance values come from the BNO055 datasheet and represent the measurement uncertainty for each sensor component. These values indicate to the EKF how much to trust each measurement during fusion; higher covariance means the filter trusts that measurement less. In robot_localization, these covariances remain static throughout the operation, so getting them right at the start matters.

The broadcaster spawns alongside the other controllers in the launch file:

imu_broadcaster_spawner = Node(
    package='controller_manager',
    executable='spawner',
    arguments=['imu_sensor_broadcaster', '-c', '/controller_manager'],
)

# Delayed to ensure controller manager is ready
delayed_base_controller_spawner = TimerAction(
    period=2.5,
    actions=[base_controller_spawner, imu_broadcaster_spawner],
)

https://www.youtube.com/watch?v=MdPBkdxYOkc

EKF using robot_localization

For LeKiwi, I use the robot_localization EKF node in lekiwi_navigation to fuse wheel odometry with IMU data. The integration centers on a key design choice: the wheel controller provides velocities in the body frame, while the IMU provides drift-free heading. The EKF combines these to produce a corrected odometry estimate.

First, I disable TF publishing from base_controller so the EKF owns the odom → base_footprint transform:

# controller_config.yaml
base_controller:
  ros__parameters:
    enable_odom_tf: false

The controller still publishes /base_controller/odom as a topic, but only the EKF publishes the transform.

From wheel odometry, I fuse only body-frame velocities (vx, vy). Position and heading are deliberately excluded - the controller computes them by dead reckoning, which drifts when the robot gets bumped or manually rotated. By giving the EKF velocities instead, the filter integrates them using the IMU's correct heading:

odom0_config: [false, false, false,  # position - excluded
               false, false, false,  # orientation - excluded
               true,  true,  false,  # vx, vy - fused
               false, false, false,  # angular velocities - excluded
               false, false, false]  # accelerations - excluded

From the IMU, I fuse absolute orientation (roll, pitch, yaw) and angular rates. Linear acceleration is excluded - motor vibration injects too much noise, and wheel velocities are already cleaner:

imu0_config: [false, false, false,  # position - excluded
              true,  true,  true,   # roll, pitch, yaw - fused
              false, false, false,  # linear velocities - excluded
              true,  true,  true,   # angular velocities - fused
              false, false, false]  # linear acceleration - excluded

The filter runs at 50 Hz to match the controller_manager, and I configure it for 3D operation (two_d_mode: false) so it tracks roll and pitch, which is not necessary at the moment but would be useful on uneven surfaces:

frequency: 50
two_d_mode: false
sensor_timeout: 0.1
imu0_differential: false  # BNO055 NDOF gives global orientation
imu0_remove_gravitational_acceleration: false  # BNO055 firmware already strips gravity

For debugging, the stack supports three launch modes: normal (fuses both sources), IMU-only (bench testing when wheels spin freely), and odometry-only (verifies encoders in isolation, auto-enables in mock mode). Even in single-sensor modes, the EKF still runs - it's not fusing anything, but this keeps TF ownership consistent across all modes rather than conditionally enabling TF from the individual sensor nodes:

ros2 launch lekiwi_bringup lekiwi.launch.py # Normal (default)
ros2 launch lekiwi_bringup lekiwi.launch.py imu_only:=true
ros2 launch lekiwi_bringup lekiwi.launch.py odom_only:=true

Validation and testing

The fusion pipeline (focusing on the IMU) looks like this:

BNO055 (I2C) 
  → bno055_hardware_interface 
    → imu_sensor_broadcaster 
      → robot_localization EKF (also includes wheel odometry)
        → /odometry/filtered + odom→base_footprint TF

I tested the sensor fusion by manually driving the LeKiwi around a loop, rotating it while stationary, and giving it gentle bumps during motion. Meanwhile, I monitored the /base_controller/odom and /odometry/filtered side by side on Foxglove. This is by no means perfect, and still needs a SLAM implementation on top to provide the map to odom transform. Still, as internal odometry goes, the sensor fusion is quite effective.

https://www.youtube.com/watch?v=PFRJnBID5ME

I'll be honest, the only real advantage of the IMU is to detect external impacts such as bumps or manual pushes when not using SLAM. In the future, it should also be useful to detect motion over bumps and on ramps, since the IMU measures in 9 DoF.

The wheel odometry by itself is quite impressively accurate, so it can dead reckon very well. If I add a SLAM method directly on top of the wheel odometry, it will still be able to correct the localization in case of external movement. So technically, on flat surfaces, fusing the IMU is not necessary, but it is definitely a fun learning experience. And it gave me a chance to experiment with vibe-coding hardware interfaces, which was quite enlightening.

Why robot_localization

robot_localization is technically deprecated - no active roadmap, though community PRs still land, even in the latest ROS 2 distributions.

The proposed successor is Fuse, a graph-based optimization framework from Locus Robotics, deployed in thousands of warehouse robots globally and genuinely field-tested at scale. However, the challenge here is that Fuse is much more complex to get started with and configure. I will skip this for now.

fusioncore is newer - ROS 2-native with a 22-state UKF, automatic IMU bias estimation, adaptive noise, outlier rejection, and advanced GPS features. However, despite an impressive features list, fusioncore is unproven in real-world deployments, and its documentation contains misleading claims, especially about robot_localization's capabilities, raising concerns. I did try it out and had decent results, but did not dig deeper.

Despite the alternatives, and the somewhat promising results from fusioncore, I chose robot_localization anyway. The deciding factor: per-axis fusion selectivity. robot_localization lets me exclude specific measurement axes - fuse only wheel velocities (not position/heading) and only IMU orientation/rates (not linear acceleration), for example. fusioncore always fuses everything; you can only down-weight unwanted axes via noise inflation, which is an approximation rather than exclusion. Additionally, fusioncore's improvements over robot_localization lie in its GPS-related features, which, for the current state of LeKiwi, add no value.

For the LeKiwi use-case - indoor, no GPS, BNO055 handles calibration and sensor-fusion on-chip - robot_localization's maturity, active deployments, years of field testing, and especially the per-axis control, outweigh fusioncore's advanced features.

I will write a blog post about working with fusioncore soon, and will try to include an evaluation of Fuse as well, but that is for another day.

What's next?

The robot_localization setup covers odometry within the odom frame. The next step towards full autonomy is adding a map layer using Nav2. First, I want to go back to the pan-tilt mechanism and finish the implementation. The control part is complete, but since I've added an OAK-D S2 depth camera to the pan-tilt component, I want to utilize it fully and integrate at least a basic launch file into the LeKiwi ROS 2 stack.

The OAK-D S2 includes 2 monochrome cameras for depth/disparity calculation, an RGB camera, an on-board processor for inferencing, and an IMU. This makes it perfect for visual-inertial odometry (VIO) and visual SLAM - some fun concepts to experiment with. My final objective, however, is to eventually integrate it with my Oculus Quest 2 headset - use the head tracking data to move the pan-tilt mechanism, while streaming RGB video to the headset, and use the handheld controllers to teleoperate the LeKiwi.

I've also ended up with another robot, maybe two. I purchased an Ikea Råskog trolley to attach to the LeKiwi, inspired by the XLeRobot project. As soon as I brought it home, I realized that the middle shelf fit perfectly on top of a mecanum wheeled robot I had brought home from work. Meanwhile, I also had two spare hoverboard wheels, which looked perfect for the bottom shelf and the caster wheels - this is still a work in progress.

This mecanum robot has a Teensy 4.1 in its base, driving the motor controllers. I want to use this platform to tinker with Zenoh-Pico and Pico-ROS, as an alternative to micro-ROS. But this is for another day; for now, it's simply a regular trolley for storage, but with motors and mechanum wheels instead of caster wheels.

But that's for another time. For now, I'm back to working with the LeKiwi. If you want to poke around, here are the repos:

• lekiwi_ros2: top-level directory containing lekiwi_control, lekiwi_description, lekiwi_navigation, and lekiwi_bringup

• bno055_hardware_interface: the ros2_control sensor plugin and diagnostics node

As always, feedback and contributions are welcome!

Originally written for and published on the Foxglove blog

Context / Why a Steam Deck?

A few years ago, I wrote about using a Steam Deck as a robot controller to drive and visualize my mecanum-wheeled robot AKROS. That setup worked well, but getting ROS 2 running on the Steam Deck’s operating system required some workarounds at the time. A lot has changed since then.

https://www.youtube.com/watch?v=q8D-sAIVN-s

Last year, I retired the AKROS platform and started working on a new robot - LeKiwi - to convert it to ROS 2 and make minor hardware upgrades as I progressed. The build has progressed a lot - I covered the LiDAR integration and the camera calibration. The robot now has a complete perception stack, ROS 2 Control integration, and communicates via Zenoh and Tailscale.

With all of that in place, I wanted a portable way to drive and monitor it without hauling a laptop around, which brought me back to the Steam Deck. The idea is straightforward: use the Steam Deck’s built-in controller to drive the LeKiwi, and use Foxglove to visualize what the robot sees and does - all from a single handheld device. And since the Steam Deck is a full-fledged computer, I can even develop code on it when I need to.

Setting up ROS 2 using Pixi

When I first set up ROS 2 on the Steam Deck two years ago, I used Distrobox to create a full Ubuntu container and installed ROS 2 inside it. The Steam Deck runs on SteamOS, which is based on Arch Linux, so there is no native ROS 2 support available. It worked, but having to enter a container to run ROS commands added friction, and managing dependencies between the host and container was annoying.

This time, I used Pixi, a package manager that can install ROS 2 packages via the RoboStack project. Unlike Distrobox, Pixi does not need a container - it resolves and installs packages into a project-local directory, similar to how conda works. No separate OS image to maintain, just native binaries managed by a single pixi.toml file. I already use this to run ROS 2 on my MacBook, so extending the same workflow to the Steam Deck was straightforward.

Installing Pixi

Installation is a one-liner using the following command. Then follow this tutorial to set up your favorite ROS 2 distro (I am currently using Kilted).

curl -fsSL https://pixi.sh/install.sh | sh

Creating a ROS 2 workspace

Once Pixi is installed, setting up a ROS 2 workspace is easy:

pixi init ros2_ws -c robostack-kilted -c conda-forge
cd ros2_ws

This creates a new Pixi project where you can add any ROS 2 packages you need using pixi add <package-name>.

Setting up Foxglove

There are two ways to run Foxglove on the Steam Deck: the desktop app or the browser app. Both methods connect to your robot using the Foxglove bridge, which runs on the LeKiwi’s Raspberry Pi and exposes ROS 2 topics over a WebSocket connection. The bridge is the only additional piece needed on the robot side.

Option 1: Desktop app via Distrobox

Foxglove is not available on the Pixi registry, and there is no native Arch Linux package for it either. So for the desktop app, Distrobox is still my preferred option.

First, install Podman and Distrobox, then create and enter an Ubuntu container:

sudo pacman -S podman distrobox
distrobox create --name ubuntu24 --image ubuntu:24.04
distrobox enter ubuntu24

Inside the container, install Foxglove following the official instructions:

sudo apt update && sudo apt install foxglove-studio libasound2t64

The trick I learned this time is to export it as a host application, so you do not have to enter the container every time. This creates an executable on SteamOS that you can pin to the desktop or taskbar:

distrobox-export --app foxglove-studio

Option 2: Browser app

If you would rather skip the Distrobox setup entirely, Foxglove also has a browser-based version. No installation or workarounds - just open it in the Steam Deck’s browser and connect to the Foxglove bridge running on the robot. In my testing, both options performed about the same, though I did not push either to its limits. I ended up using the desktop app since I already had Distrobox set up, but the browser option is a good alternative if you want to avoid that overhead entirely.

Controller mapping

From the Steam Deck settings, you can map the controller inputs to buttons and axes of a game controller in desktop mode. I reused the mapping I used with my previous robot: trackpads and back buttons as desktop mouse controls, and the remaining buttons mapped to a standard gamepad.

With this or any other mapping, you can run the default joy node without any special configuration.

Joy node setup

One of the nice things about the Pixi setup is how easy it is to add ROS 2 packages. Getting the joy node running is as simple as:

pixi add ros-kilted-joy

After that, you can launch it using:

pixi shell
ros2 run joy joy_node

# OR
pixi run ros2 run joy joy_node

The joy node reads input from the controller and publishes sensor_msgs/msg/Joy messages, which can then be consumed on the LeKiwi side to drive the robot.

You can also create a task in your pixi.toml:

[tasks]
joy = "ros2 run joy joy_node"

With this, a single pixi run joy command is all you need for teleoperation.

Auto-start on boot

To take this one step further, I created a systemd service that automatically runs joy when the Steam Deck boots up and kills the process on shutdown:

[Unit]
Description=Joy Node (pixi run joy)
After=network-online.target
Wants=network-online.target

[Service]
Type=simple
User=deck
WorkingDirectory=/home/deck/ros2_ws
ExecStartPre=/bin/sleep 15
ExecStart=/home/deck/.pixi/bin/pixi run joy
ExecStop=/bin/bash -c 'pkill -f "lib/joy/joy_node" || true'
Restart=on-failure
RestartSec=10

[Install]
WantedBy=multi-user.target

To enable the service:

sudo cp joy.service /etc/systemd/system/joy.service
sudo systemctl daemon-reload
sudo systemctl enable joy.service
sudo systemctl start joy.service

Now, when the Steam Deck turns on, it immediately starts publishing Joy messages. I would not recommend it if you have more than one robot on your network, but for a single robot, it is not a problem.

Integration and testing

With everything in place - connectivity between the Steam Deck and the robot’s Raspberry Pi, the joy node reading controller input, and Foxglove visualizing data - it was time to put it all together.

My workflow looks like this:

1. I start up the Steam Deck in desktop mode, which automatically launches the joy node via the systemd service. On the LeKiwi side, the Raspberry Pi is already running its ROS 2 stack with the Foxglove bridge (I run these manually for now, but I am considering adding them as services on the robot as well).

2. I open the Foxglove app on the Steam Deck, connect to the bridge, and I immediately see live sensor data - LiDAR scans, camera feeds, and the robot model in 3D (the meshes do take about a minute to load).

3. Then I can just pick up the Steam Deck and drive. The built-in analog sticks control the robot’s omnidirectional movement, and I can visualize everything on Foxglove at the same time.

https://www.youtube.com/watch?v=TPuJHzK34iQ

My layout shows only the 3D panel, which displays the robot model with live LiDAR scans, the image stream, and the URDF.

Wrapping up

The Steam Deck was already quite a capable platform for robotics work, but with the added service and tools like Pixi, my workflow has become significantly easier. Adding Foxglove to the mix reveals exactly what the robot sees and does as I drive it around. I can simply turn on the Steam Deck, and in a few clicks, I am ready to drive the robot and visualize.

This setup has now become my go-to for testing and teleoperating the LeKiwi. It is portable, self-contained, and requires zero extra hardware. Additionally, with Tailscale for networking and Zenoh as middleware, I do not need to worry about communication anymore.

My work on the ROS 2-powered LeKiwi is available on GitHub, but please note that this is an active project, so expect rapid and sometimes breaking changes.

This post walks through what went wrong, how I tracked down the root cause, the fix, and how I tested it on a new build - a Pan-Tilt mechanism, a two DoF pan-tilt mechanism built from SO-100/101 parts.

The Problem

The STS3215 uses a 12-bit magnetic encoder, giving 4096 discrete steps (0–4095) per revolution. The motor counts clockwise, but ROS 2 follows REP-103, where positive angles are counterclockwise. This means the hardware interface must invert direction when converting between raw steps and radians.

Here’s how the original conversion functions worked:

// Read: motor steps → radians
inline double raw_position_to_radians(int raw_position)
{
  return static_cast<double>(STS_MAX_POSITION - raw_position) * STEPS_TO_RAD;
  // STS_MAX_POSITION = 4095, STEPS_TO_RAD = 2π / 4096
}

// Write: radians → motor steps
inline int radians_to_raw_position(double position_rad)
{
  double normalized = std::fmod(-position_rad, 2.0 * M_PI);
  if (normalized < 0.0) {
    normalized += 2.0 * M_PI;
  }
  int raw = static_cast<int>(normalized * RAD_TO_STEPS);
  return std::clamp(raw, 0, STS_MAX_POSITION);
}

This mapping only works for joints that move in the [0, 2π] range. But when you set joint limits to [-π, π], a few problems appear:

1. Position feedback is always positive

raw_position_to_radians always returns values in [0, 2π] regardless of joint limits, which only clamp outgoing commands. A servo at the center reports π rad instead of 0.

2. Negative commands take the long way around

Let's say you give the command to move -0.1 rad, the servo should move a small amount in the clockwise direction, around 65 encoder steps. Since 0 rad sits at step 4095, the servo travels 4030 steps to get to step 65 — nearly a full revolution in the wrong direction.

3. The read/write round-trip was broken

I also discovered that the two functions weren’t true inverses:

So, writing 0 radians gives step 0, but reading step 0 returns 2π radians. The functions disagree about which end of the encoder is 0.

All three problems share the same root cause.

Root Cause

Two formulas, neither being algebraic inverses of each other:

• Read: step 4095 is 0 rad

• Write: step 0 is 0 rad

For all positive angles strictly inside (0, 2π), both formulas produce results that look plausible, so the inconsistency is hidden during normal testing. The bug only surfaces at the boundaries and for negative angles.

The Fix: A Configurable Center

To fix this, a single unified formula with a configurable center parameter was implemented, with the center parameter being the encoder step that maps to 0 rad:

constexpr int STS_DEFAULT_CENTER = 4095;

inline double raw_position_to_radians(int raw_position, int center = STS_DEFAULT_CENTER)
{
  return static_cast<double>(center - raw_position) * STEPS_TO_RAD;
}

inline int radians_to_raw_position(double position_rad, int center = STS_DEFAULT_CENTER)
{
  double steps = std::round(static_cast<double>(center) - position_rad * RAD_TO_STEPS);
  return static_cast<int>(std::clamp(steps, 0.0, static_cast<double>(STS_MAX_POSITION)));
}

Values outside the encoder's physical range are now clamped to [0, 4095] rather than wrapped with fmod. This default center=4095 preserves existing behavior, while fixing the bugs.

How to Use It

Set position_center_steps to the encoder step (0–4095) that maps to the center position, or 0 rad for each joint:

<ros2_control name="my_hardware" type="system">
  <hardware>
    <plugin>sts_hardware_interface/STSHardwareInterface</plugin>
    <param name="serial_port">/dev/ttyACM0</param>
    <!-- ... -->
  </hardware>
  <joint name="pan_joint">
    <param name="motor_id">1</param>
    <param name="operating_mode">0</param>
    <param name="position_center_steps">2048</param>
    <!-- ... -->
    <command_interface name="position"/>
    <state_interface name="position"/>
  </joint>
</ros2_control>

Set it to 2048 for a symmetric [-π, +π] range, or 4095 for [0, 2π]. Values outside the [0, 4095] range are rejected at startup. The Pan Tilt 100 uses this exact parameter: 2048 for the pan axis and 2646 for the tilt axis, which is calibrated to that motor's physical resting position.

Note that this only applies in position mode (operating_mode=0), it is ignored with a warning on velocity or PWM mode joints.

What Changes for Existing Users?

Almost nothing, the center position defaults to 4095, matching the old behavior for all non-zero inputs. All other angles are unaffected. To see this working on real hardware, the next section walks through the Pan Tilt 100 build.

Real-World Example: Pan Tilt 100

The original plan was to attach a SO100 arm to the LeKiwi. I had some spare motors, so I decided to assemble an arm, and I started printing fresh parts in white PLA. Unfortunately, I ran out of filament halfway through. While waiting for more filament to arrive, it clicked: two motors are all I need to test the hardware interface in position mode, and it also works perfectly as a pan-tilt mechanism. So, I stopped the arm build right there, bolted an OAK-D S2 to the second link with a 3D printed mount, and inspired by the SO100, called it the PT100, where PT stands for Pan Tilt.

The camera integration - VIO, VSLAM, and whatever else follows - is a topic for another post. Getting the depthai-ros wrapper to work was its own adventure nightmare.

URDF and hardware configuration

The PT100 has two joints — pan_joint and tilt_joint, both running in position mode through sts_hardware_interface. The relevant part of the control xacro:

<ros2_control name="pt100_control" type="system">
  <hardware>
    <plugin>sts_hardware_interface/STSHardwareInterface</plugin>
    <param name="serial_port">/dev/ttySERVO</param>
    <param name="use_sync_write">true</param>
    <param name="max_velocity_steps">3400</param>
  </hardware>

  <joint name="pan_joint">
    <param name="motor_id">1</param>
    <param name="operating_mode">0</param>
    <param name="min_position">-1.5708</param>
    <param name="max_position">1.5708</param>
    <param name="max_velocity">4.43</param>
    <param name="position_center_steps">2048</param>
    <command_interface name="position"/>
    <state_interface name="position"/>
    <!-- velocity, effort, voltage, temperature, current, is_moving -->
  </joint>

  <joint name="tilt_joint">
    <param name="motor_id">2</param>
    <param name="operating_mode">0</param>
    <param name="min_position">-1.5708</param>
    <param name="max_position">1.5708</param>
    <param name="max_velocity">4.43</param>
    <param name="position_center_steps">2646</param>
    <command_interface name="position"/>
    <state_interface name="position"/>
    <!-- velocity, effort, voltage, temperature, current, is_moving -->
  </joint>
</ros2_control>

The pan joint was assembled at its original mid-point, and uses position_center_steps: 2048, providing a symmetric [-π/2, +π/2] range. The tilt joint uses position_center_steps: 2646, calibrated to its physical resting position, which isn't at the exact midpoint due to how the link is assembled.

max_velocity is capped at 85% of the hardware max (3400 steps/s → 2890 steps/s ≈ 4.43 rad/s), leaving headroom for the motor to decelerate without overshooting.

The controller is a ForwardCommandController on the position interface, running at 50 Hz. With use_sync_write: true, both joints are commanded in a single serial transaction each cycle.

One non-obvious URDF detail: both joints set velocity="1e6" in their <limit> elements and has_velocity_limits: false in the controller config. joy_teleop sends absolute position targets, so when the joystick moves quickly, the commanded jump can be nearly π rad in a single cycle. ros2_control's JointSaturationLimiter would clamp that to the URDF velocity limit and log spurious "command out of limits" errors every cycle. Setting the limit to a physically unreachable value disables that clamping without touching position enforcement. The sts_hardware_interface still caps actual motor speeds via max_velocity.

For manual control, I use the Steam Deck's gamepad with joy and joy_teleop: left stick for pan/tilt (±π/2 rad at full deflection), L1 as a deadman switch. The A and B buttons call the /emergency_stop service — provided by sts_hardware_interface to enable or disable a hard stop independently of the deadman switch.

https://www.youtube.com/watch?v=avtmbQLGMdY

The fix in action

The results, including plots showing joint command tracking is shown in the video below, visualized on Foxglove. For some time, you also see the input command plot changing, but the joint states don't move; this is because I had pressed the emergency stop at that time.

https://www.youtube.com/watch?v=lExx9SFCsuE

E-Stop and out-of-range positions

During testing, I ran into a second issue, separate from the conversion bug.

When the e-stop is active, torque is disabled, and both joints can be moved freely by hand. If a joint drifts, maybe by gravity, or it is deliberately pushed past its URDF-defined limits while torque is off, ros2_control's JointSaturationLimiter detects the out-of-bounds position the moment the e-stop is released and immediately deactivates the controller. No warning or recovery path, the controller just stops.

The fix: always clamp the position reported by read() to within the URDF limits whenever limits are defined:

if (has_position_limits_[i]) {
  hw_state_position_[i] = std::clamp(hw_state_position_[i], position_min_[i], position_max_[i]);
}

ros2_control now always sees a position within bounds. The write path already clamps commands to the same range, so on the first cycle after e-stop release, the motor is commanded to the nearest limit and moves back into range, while the controller stays active throughout.

Takeaways

• If two conversion functions are supposed to be inverses, always test that they actually are. A simple round-trip test (read(write(x)) ≈ x) would have caught this bug early.

• Be cautious with fmod for angle wrapping in servo code. It can silently map valid negative commands into the wrong part of the encoder range.

• When hardware can move outside software limits — during e-stop, power loss, or manual handling — clamp the reported state unconditionally before it reaches ros2_control. Otherwise, this could deactivate the controller, which needs manual intervention to reactivate.

• By defaulting to backward-compatible behavior (center=4095) and exposing the new parameter, existing deployments like lekiwi_ros2 running in velocity mode or position mode, motors operating in the [0, 2π] range are unaffected.

You can find these changes in sts_hardware_interface. If you use STS3215 servos in position mode and need negative angles, set position_center_steps to your servo’s calibrated center position. The Pan-Tilt implementation can be found in the pantilt_ros2 repository.

Alongside these fixes, the recently merged PR #10 adds a ready-to-use single-motor position mode example, including a URDF, controller config, launch file, and integration tests. You can test the new parameter with use_mock:=true before connecting any hardware.

What’s Next?

With the hardware interface bug fixed and the Pan Tilt 100 running smoothly in position mode, the next steps are to integrate with lekiwi_ros2. I changed my mind about the SO100 arm; I prefer having a pan-tilt mechanism with an OAK-D S2 depth camera instead. The LeKiwi build is now finally taking shape, moving from a mobile manipulator build to a complete mobile perception platform.

I've used this sensor before, and I normally use it with the bno055 ROS 2 (Python) package, but I had recently learned about the SensorInterface and the imu_sensor_broadcaster controller in ros2_control while developing the sts_hardware_interface package, and wanted to try it out. But I got busy with another project, so I decided to give vibe-coding a shot for this one.

The sts_hardware_interface package, although AI-assisted, was meticulously designed, reviewed, and tested manually. With the new bno055_hardware_interface, I just gave Claude (Sonnet 4.6, using a mix of Claude Code and GitHub Copilot) a bunch of references, including sts_hardware_interface, sensor datasheets, and ros2_bno055_sensor, a ROS 2 package I found, which is written in C++. I then asked it to write me a hardware interface using these references, with occasional guidance along the way. I mostly just focused on my other project.

I did use some Claude skills to do this: in-built skills like /simplify, review skills from coderabbit, and planning skills from superpowers to name a few. I didn't touch the code once. I let the AI agents do everything, from checking whether the sensor was detected over I2C, to running the ROS 2 example and executing the AI-generated tests. After a few iterations and brief manual code reviews, I had it working as expected.

I even asked Claude to write the documentation. I don't mind sloppy code, but I absolutely hate wrong documentation, so I manually verified that the docs matched the implementation.

How it works

The bno055_hardware_interface is a ros2_control SensorInterface plugin that connects ROS 2 to the BNO055 over I2C. Instead of a standalone node that publishes IMU data directly, it plugs into the ros2_control ecosystem — orientation, angular velocity, and linear acceleration are exposed as typed state interfaces consumed by the imu_sensor_broadcaster. Same idea as the sts_hardware_interface, except that one is a SystemInterface (reads and commands actuators) while this one is a SensorInterface (read-only).

The BNO055 runs sensor fusion on-chip, so the host just reads already-fused values. While the sensor supports multiple modes, I limited the hardware interface to three fusion modes, selectable via the sensor_mode launch parameter: NDOF (9-DOF absolute, default), NDOF_FMC_OFF (9-DOF without fast magnetometer calibration), and IMUPLUS (6-DOF, no magnetometer). The IMU normally self-calibrates, but the package also supports calibration persistence: save self-calibrated offsets to a YAML file and reload them at boot.

Most of the work happens in bno055_hardware_interface.cpp, which implements the SensorInterface lifecycle and exposes state interfaces for orientation, angular velocity, and linear acceleration. on_configure handles I2C init, calibration loading, axis remapping, and fusion mode activation. read() updates the state interfaces with scaled sensor data each control loop tick. A mock mode (enable_mock:=true) skips all I2C operations for testing without hardware.

The axis remapping in on_configure is worth mentioning: the BNO055 has 8 predefined placement configurations (P0–P7, per datasheet §3.4) that remap sensor axes to match your mounting orientation, so you don't have to rotate data in software. I used P1, which lines up with the axis labels on the PCB silkscreen.

The plugin uses the official Bosch BNO055 C driver and a thin I2C adapter borrowed from ros2_bno055_sensor. The config/ folder includes a URDF xacro, an imu_sensor_broadcaster configuration, and a template for calibration offsets.

There are also two optional companion nodes. bno055_diagnostics.cpp publishes sensor health (calibration status per sub-sensor, system errors, temperature) to /diagnostics at 1 Hz. imu_tf_broadcaster.cpp republishes IMU orientation as a world → base_link TF transform so that rotations can be visualized in RViz or Foxglove. The video below demonstrates this. However, I don't intend on using this on the LeKiwi since the transform is provided by fusing the IMU data with wheel odometry in the robot's navigation stack (more on this in a future post).

https://youtu.be/MdPBkdxYOkc?si=vfwHCUjZUXJJPjQN

The launch file brings up robot_state_publisher, ros2_control_node, the IMU broadcaster, and the companion nodes (optional; these can be disabled), with hardware parameters exposed as launch arguments.

Wrapping up

After all this, I ended up with a functional, tested package with features I probably wouldn't have bothered implementing myself: mock mode, diagnostics, calibration persistence, and three fusion modes. Would I vibe-code something safety-critical? Probably not. But for a sensor interface on a hobby robot, the trade-off made sense.

The bno055_hardware_interface is available on GitHub. My work on the ROS 2-powered LeKiwi is also online in the lekiwi_ros2 package, expect rapid and sometimes breaking changes as it is in active development. As always, I welcome feedback and contributions.

UPDATE: I was in a meeting at work with some GitHub employees, and I learned about a new term - Spec-Driven Development, or SDD. It turns out that was kind of what I did - get Claude to write a spec for me and then have it implemented. So, I guess this hardware interface is not vibe-coded, then? Either way, please use with caution, especially in safety-critical applications.

Setting up ros2_control in the URDF

If you've worked with ros2_control before, the first step is always to set up the URDF. I can write an entire post about it, but to keep this post short, I will direct you to the ros2_control workshop from ROSCon 2022. This is a perfect primer to get started with ros2_control, and personally, a great reference guide that I keep coming back to.

The URDF block

The <ros2_control> block in the URDF tells the controller manager which hardware plugin to load and what interfaces each joint exposes. The controller manager is the central node of the ros2_control framework, and manages the lifecycle of the controllers and handles access to the hardware interfaces.

<ros2_control name="lekiwi_base" type="system">
  <hardware>
    <plugin>sts_hardware_interface/STSHardwareInterface</plugin>
    <param name="serial_port">$(arg serial_port)</param>
    <param name="baud_rate">${baud_rate}</param>
    <param name="use_sync_write">${use_sync_write}</param>
    <param name="enable_mock_mode">$(arg use_mock)</param>
    <param name="max_velocity_steps">${sts_max_velocity_steps}</param>
    <param name="proportional_acc_max">${proportional_acc_max}</param>
    <param name="proportional_acc_deadband">${proportional_acc_deadband}</param>
  </hardware>
  <!-- Left wheel joint (Motor ID 7) -->
  <joint name="left_wheel_joint">
    <param name="motor_id">${left_motor_id}</param>
    <param name="operating_mode">${operating_mode}</param>
    <command_interface name="velocity"/>
    <command_interface name="acceleration"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
    <state_interface name="effort"/>
    <state_interface name="voltage"/>
    <state_interface name="temperature"/>
    <state_interface name="current"/>
    <state_interface name="is_moving"/>
  </joint>
  <!-- back_wheel_joint (Motor ID 8) and right_wheel_joint (Motor ID 9) follow the same pattern -->
</ros2_control>

Let's talk about the parameters.

• baud_rate, xxx_motor_id, and operating_mode are self-explanatory.

• use_mock enables the controller in mock mode, which has been explained in detail in the sts_hardware_interface post.

• use_sync_write batches all three velocity targets into a single packet instead of three sequential commands. This results in lower latency, and all wheels get their updated targets at the same time.

• sts_max_velocity_steps is the maximum rated velocity of the motor in steps. I am using STS3215, but other STS series motors have different speed ratings.

• We will come to the proportional_acc_max and proportional_acc_deadband parameters in a bit.

None of these parameter values are hardcoded. They're all xacro:property definitions in base.control.xacro (in lekiwi_description), so the main URDF stays readable, and all the tunable numbers are in one place.

<xacro:property name="serial_port" default="/dev/ttySERVO"/>
<xacro:property name="baud_rate" default="1000000"/>
<xacro:property name="use_mock" default="false"/>
<xacro:property name="use_sync_write" default="true"/>
<xacro:property name="sts_max_velocity_steps" value="3400"/>

<xacro:property name="proportional_acc_max" value="25"/>
<xacro:property name="proportional_acc_deadband" value="0.05"/>
  
<xacro:property name="left_motor_id" value="7"/>
<xacro:property name="back_motor_id" value="8"/>
<xacro:property name="right_motor_id" value="9"/>
<xacro:property name="operating_mode" value="1"/>

Note that there are two velocity limits. sts_max_velocity_steps = 3400 is the STS3215 hardware maximum, used as a hard clamp on every write. max_velocity_steps is the self-defined working limit, set to 85% of the rated maximum, for two main reasons:

1. First, I don't trust the unbranded LiPo that came with the LeKiwi kit (no datasheet, the QR code goes to an app store). So, best to limit the current draw.

2. At full speed, the motors start missing their velocity targets, which causes odometry to drift. This 85% keeps them tracking.

From max_velocity_steps, the joint limits and other values in control.yaml (in lekiwi_control) cascade automatically; change one value and everything updates.

<xacro:property name="max_velocity_steps" value="${int(sts_max_velocity_steps * 0.85)}"/> 
<xacro:property name="max_velocity_rads" value="${max_velocity_steps*2*pi/4096}"/>

<!-- Maximum allowable linear velocity per axis -->
<xacro:property name="max_linear_velocity_x" value="${wheel_radius * max_velocity_rads / (sqrt(3)/2)}"/>
<xacro:property name="max_linear_velocity_y" value="${wheel_radius * max_velocity_rads / 1.0}"/>
<xacro:property name="max_angular_velocity_z" value="${wheel_radius * max_velocity_rads / wheel_base_offset}"/>

Finally, each wheel joint, after declaring its motor ID and operating mode, declares which interfaces it exposes. The hardware interface reads back position, velocity, effort, voltage, temperature, current, and is_moving as state interfaces every cycle. All is defined in the hardware interface; no extra custom code needed.

Configuring the Controllers

The control configuration is defined in lekiwi_control , which is mostly YAML and launch files, no code. And the configuration is quite simple: just two controllers, running at 50 Hz.

controller_manager:
  ros__parameters:
    update_rate: 50 # Hz
    statistics_publish_rate: 0.0  # Disable /controller_manager/statistics/* topics
    
    # Command and error handling
    enforce_command_limits: true  # Enforce URDF joint limits via JointSaturationLimiter
    handle_exceptions: true       # Graceful error handling
    
    # Hardware component initialization
    defaults:
      switch_controller:
        strictness: strict
      allow_controller_activation_with_inactive_hardware: false
      deactivate_controllers_on_hardware_self_deactivate: true

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    base_controller:
      type: omni_wheel_drive_controller/OmniWheelDriveController

joint_state_broadcaster:
  # joint state broadcaster parameters...

base_controller:
  # omni wheel drive controller parameters...

Key takeaways:

• enforce_command_limits: true tells the controller manager to run ros2_control's JointSaturationLimiter, which clamps commands to the URDF velocity limits before they reach the hardware.

• JointStateBroadcaster publishes all state interfaces to /joint_states and /dynamic_joint_states. Temperatures, currents, voltages, stall flags: all of it is accessible using standard ROS 2 tools.

• motor_diagnostics_node subscribes to /dynamic_joint_states and fires warnings when things go out of range, but that's a separate post for another time.

• OmniWheelDriveController handles the omni-wheel kinematics using the robot's geometry as parameters. By setting open_loop: false, the controller measures wheel velocities for odometry rather than commanded targets.

https://www.youtube.com/watch?v=cCn6vuQvdrg

In this video, you can see the robot being driven around as the dynamic joint state topic values update in real time.

Teleoperation

A single teleop_twist_joy node converts incoming joystick inputs (from the joy package) to velocity commands and publishes to /base_controller/cmd_vel.

One thing worth flagging: the parameter publish_stamped_twist must be set to true. The OmniWheelDriveController subscribes to TwistStamped, not plain Twist. Publish bare Twist, and the robot does nothing, which is annoying to debug.

UPDATE: I switched from teleop_twist_joy to joy_teleop instead. joy_teleop allows me to call ROS 2 services as well, which allowed me to configure buttons for enabling and disabling the emergency stop. The normal teleoperation works identically to teleop_twist_joy. I have also reused the same config file for this switch.

The configuration is defined in the teleop.yaml file in lekiwi_control.

https://www.youtube.com/watch?v=wDOlI_AGhKk

Do note that I'm not using this teleop method at the moment. Since I'm working remotely on my robot, I'm using a vibe-coded teleop extension on Foxglove instead. It still has some issues, and once I fix them, I'll write a separate post about it.

Fine-Tuning

Fine-tuning the control system took a few sessions and unearthed some issues. Let me start with the biggest problem first.

The Rotation Problem

This was a confusing one. Every time I tried to move the robot linearly in the Y direction (strafing), the robot rotated a little bit. Let go of the strafe button, and the robot twitched back.

https://youtu.be/I22CLfwls-g

Turns out the cause of this is the three-wheel geometry. For a purely sideways strafe, each wheel gets a different target velocity.

ω_back  = +vy / r         (full change, back wheel at 180°)
ω_left  = −vy / (2r)      (half change, 60° projection)
ω_right = −vy / (2r)      (half change, 300° projection)

This means that the back wheel has to reach twice the speed of the front two. Assuming we are starting from zero, the back wheel has twice the velocity delta to cover. At the time, the ACC register of the motors was set to 0 in sts_hardware_interface. This does not mean zero acceleration; this means zero ramp - all motors just chase their targets as fast as they physically can. So, by the time the front wheels have reached their targets, the back wheel is still catching up, so the speed ratios are all wrong and the chassis rotates. Same thing on deceleration, in reverse.

Synchronised motor ramps

The most straightforward fix is to use the motor's ACC register differently. Instead of sending the same ACC = 0 to all three wheels, sts_hardware_interface now sets it proportionally: the wheel with the biggest speed change gets the highest ACC, the others get lower values. Since ramp time ≈ Δv / ACC, this guarantees equal ramp durations: if one wheel's Δv is twice another's, its ACC is twice as high, so both finish in the same time. All three arrive together:

// From sts_hardware_interface write() — SyncWriteSpe path
double max_delta_rad_s = 0.0;
for (size_t j = 0; j < velocity_motor_indices_.size(); ++j) {
    size_t idx = velocity_motor_indices_[j];
    double target = conversions::apply_limit(hw_cmd_velocity_[idx], ...);
    velocity_sync_velocities_[j] = conversions::rad_s_to_raw_velocity(target, max_velocity_steps_);
    velocity_sync_deltas_[j] = std::abs(target - hw_state_velocity_[idx]);
    max_delta_rad_s = std::max(max_delta_rad_s, velocity_sync_deltas_[j]);
}
for (size_t j = 0; j < velocity_motor_indices_.size(); ++j) {
    if (max_delta_rad_s < proportional_acc_deadband_rad_s_) {
        velocity_sync_accelerations_[j] = 0;  // tiny step — glide to target
    } else {
        int acc = static_cast<int>(
            std::round((velocity_sync_deltas_[j] / max_delta_rad_s) * proportional_acc_max_));
        velocity_sync_accelerations_[j] = static_cast<u8>(std::clamp(acc, 1, STS_MAX_ACCELERATION));
    }
}
servo_->SyncWriteSpe(ids, n, velocities, accelerations);

Two parameters in base.control.xacro tune this:

• proportional_acc_max: This is the acceleration set for the motor with the highest delta velocity. The other motors will get ACC values that are proportional to this. The range is [0, 254]

• proportional_acc_deadband: This value defines a deadband, which stops the ramp from firing on tiny corrections. If a wheel delta is within this tolerance value, then ACC=0.

Even after this change, the rotation issue doesn't disappear completely; that's a geometry property you can't tune away. But with this fix, and the lower velocity/acceleration limits (covered next), the rotation is minimized significantly.

Velocity Limits

There are three layers of velocity limits, each serving a different purpose:

• Hardware ceiling (sts_max_velocity_steps = 3400): absolute motor maximum, straight from the motor's datasheet. The motor cannot physically go past this limit.

• Operating limit (max_velocity_steps = sts_max_velocity_steps * 0.85): 85% of the physical ceiling, allowing for some headroom, and cascades into base_controllers.yaml.

• Controller limits (defined in base_controllers.yaml): deliberately lower than the joint operating limits, and the reason will become clear in a bit.

When I originally calculated the joint limits in SI units based on the operating limit in steps, I did it one axis at a time. But in real life, a mobile robot, especially a holonomic one like the LeKiwi, rarely moves one axis at a time. It is usually a combination of axes in motion at any one time. My original calculation was 0.226 m/s for linear Y and 1.71 rad/s for yaw. But at max strafe + max rotation, the back wheel needs:

ω_back = (vy + R × ωz) / r = (0.226 + 0.132239 × 1.71) / 0.051 = 8.86 rad/s - 2× the joint limit

When enforce_command_limits clips one wheel but not the others, the speed ratios break, and once again, the robot rotates. Same symptom as before, different cause. The fix is to set the controller limits low enough that any multi-axis combination stays within the joint limit

That means lower single-axis ceilings: lateral speed drops from 0.226 m/s -> 0.13 m/s, rotation from 1.71 rad/s -> 0.44 rad/s. The teleop scales match exactly, so full joystick deflection reaches the limit (~4.43 m/s) without breaking it.

Per-Wheel Acceleration

There are two separate acceleration controls, and it's worth being clear about which does what.

The hardware ACC is handled by the proportional scheme explained above. The controller leaves the acceleration interface at zero and sts_hardware_interface updates it every cycle based on each wheel's velocity delta. A big step means high ACC on the back wheel and lower on the front two; a small nudge might not even clear the deadband, and the motors just glide with ACC = 0.

The max acceleration in base_controllers.yaml is a different thing altogether. It limits how fast the speed target is allowed to change, before wheel speeds are even calculated:

linear:
  x:
    max_velocity: 0.13
    max_acceleration: 0.65   # m/s²  — ~200 ms ramp to full speed
  y:
    max_velocity: 0.11
    max_acceleration: 0.55   # m/s²  — ~200 ms ramp to full speed
angular:
  z:
    max_velocity: 0.44
    max_acceleration: 2.2   # rad/s² — ~200 ms ramp to full speed

At 50 Hz, 0.55 m/s² gives a per-cycle step of 0.011 m/s (0.216 rad/s at the back wheel), well above the 0.05 rad/s deadband. That matters: set max_acceleration too low and the per-cycle step falls below the threshold, ACC stays at zero, and the back wheel races to its target unchecked. And now you're back to the rotation problem.

https://youtu.be/M--6u0VrTXU

I know this sounds quite confusing; it took me nearly a day to wrap my head around what's happening. But once I set the new velocity and acceleration values, a lot of my issues were resolved, as you can see in the video. Yes, the robot is significantly slower, but there are no surprises in its motion anymore, and no "out of limits" errors.

What's Next?

Next up is integrating my SO-100/101 arm with the LeKiwi base: position-mode arm joints and a PWM/effort-controlled gripper alongside the velocity-mode wheels. That's the mixed-mode case sts_hardware_interface was designed for, and a good real-life stress test for my implementation.

In parallel, I also want to get into autonomy for the mobile base; the sensor hardware (LiDAR and Camera) and ROS 2 driver stack are ready.

If you want to poke around with the packages mentioned in this article, here are the GitHub links:

• SCServo_Linux: the C++ SDK for controlling Feetech motors. I didn't mention it explicitely, but it is very important for sts_hardware_interface

• sts_hardware_interface: the hardware interface implementation

• lekiwi_ros2: top-level directory that contains the following packages

  • lekiwi_bringup: bringup package with configs and launch files

  • lekiwi_control: control-related package with configs, nodes, and launch files

  • lekiwi_description: URDF and meshes for LeKiwi

As always, feedback is welcome, PRs/contributions are really appreciated. More LeKiwi stuff (and a cool Agentic AI project) coming soon...

Originally written for and published on the Foxglove blog

Project Context

I purchased a LeKiwi mobile robot kit late last year - an omnidirectional platform originally designed for the LeRobot framework - and have been gradually rebuilding it to run autonomously with ROS 2. The first major addition was a LiDAR, which I covered in my previous post. Alongside that, I worked on the URDF and set up the motion control using ros2_control.

The next piece of the puzzle is the camera. LeKiwi includes a camera on the mobile base, and I’m using a cheap, generic webcam that fits perfectly with the mount the kit shipped with. Before it can be useful for anything beyond a live video feed, it needs to be calibrated. This post explains how I achieved this to complete the perception stack for the robot.

Why do we even need Calibration?

Every camera lens bends light slightly differently. Cheaper webcams (like the one I'm using on LeKiwi) often introduce significant barrel distortion, making straight lines appear curved, especially near the edges of the frame. This might be barely visible when streaming images, but it's a nightmare for robotics applications that require accurate depth perception, object detection, or visual odometry.

This is where camera calibration comes in. It is the process of determining the camera's mathematical properties: its focal length, optical center, and lens distortion characteristics. These “intrinsic parameters” describe how it transforms the 3D world into a 2D image, and can be used by robotics applications to mathematically correct the lens imperfections.

Camera Calibration 101

Camera intrinsics are captured in a camera matrix (also called the intrinsic matrix), which contains:

• Focal length (fx, fy): The scaling factor between 3D scene coordinates and 2D image pixels - two values to account for potentially non-square pixels

• Principal point (cx, cy): The optical center of the image (usually near the middle, but not always!)

The intrinsic matrix K is defined as a 3x3 matrix:

    [fx  0 cx]
K = [ 0 fy cy]
    [ 0  0  1]

Additionally, the calibration process generates:

• Distortion coefficients: Parameters that model radial and tangential lens distortion, depending on the distortion model used, like plumb_bob or rational_polynomial.

• Rectification matrix: Rotation matrix for aligning stereo images (for stereo cameras only)

• Projection matrix: Intrinsic matrix of the projected (rectified) images

In ROS 2, this information lives in a sensor_msgs/msg/CameraInfo message, which camera drivers publish alongside image data. Downstream nodes, such as those performing visual odometry, for instance, subscribe to both the raw image topic and the camera info topic, and correct for distortions internally.

The “official” ROS 2 calibration process

The standard approach in ROS 2 uses the camera_calibration package with a printed checkerboard pattern. Here's the workflow:

1. Print a checkerboard: You'll need a checkerboard pattern with known dimensions. Make sure it's mounted flat on a rigid surface. Calib.io has a really intuitive pattern generator that I used to print my checkerboard patterns.

1. Launch the calibration node: Run the calibrator from the camera_calibration package, specifying your checkerboard dimensions and camera topics.

2. Move the checkerboard: Move the board around to capture diverse angles - left, right, top, bottom, tilted, close, far. The calibration GUI shows progress bars that track coverage across the X, Y, and size dimensions. Hold each position until the window highlights the captured frame.

3. Calibrate and save: Once you've collected enough data, hit the Calibrate button. After processing, review the undistorted result, then save the parameters to a YAML file, which you can then load into your camera driver using the camera_info_url parameter.

The full tutorial with detailed instructions is available in the ROS documentation: ROS 2 monocular camera calibration tutorial.

How I actually did it

The official calibration process provides a GUI that needs a display, which I don't have since I'm running the LeKiwi on Ubuntu Server without a desktop GUI installed. I also cannot find my USB-A to USB-C adapter, so I can't even connect the camera to my laptop directly. So I had to get creative. I copied the camera calibration script to my laptop and modified it to run "headless".

To do this, I copied the original camera_calibration script and updated it to display coverage and progress information in the terminal rather than the GUI window. I then added the calibration and commit steps to the termination handler, so that once I had collected enough data, I could simply stop the script and it would automatically calibrate and save the parameters. This script can be found as a GitHub Gist here. It is a hacky solution, but it got the job done without needing any fancy setup.

For visual feedback, I used Foxglove and its Image Panel to visualize the camera feed, while I moved the checkerboard around in front of the camera and monitored the terminal output on the side. LeKiwi's Raspberry Pi and my laptop were on the same Wi-Fi network and using Zenoh for communication. For Foxglove, I simply started the Foxglove bridge on the Raspberry Pi and connected to it from the Foxglove app on my laptop.

What’s really happening under the hood

Both implementations share the same core calibration math. For each captured frame, OpenCV locates the checkerboard corners and then performs a non-linear least-squares optimization to find the camera matrix K and distortion coefficients D. The results are saved as a YAML file containing the camera matrix, distortion coefficients, rectification matrix, and projection matrix.

The main difference is in quality control. The official GUI tool lets you see detected corners in real-time, and expects you to hold the checkerboard still, so you naturally avoid capturing blurry or poorly-lit frames - the human is the filter. Without a display, my headless script can't do that, so it compensates algorithmically: it detects and removes outliers, then re-runs calibration on the cleaned dataset.

There are also minor differences in the OpenCV functions used: The original method uses findChessboardCorners + cornerSubPix for corner detection, my method uses findChessboardCornersSB. The original uses calibrateCamera; mine uses calibrateCameraExtended, an extended version that provides additional data, which I then use for outlier detection. That said, the underlying math is identical.

The detailed explanation about camera calibration can be found in the OpenCV documentation here.

Spot the difference: before and after calibration

With the calibration file in hand, it's time to put it to use. I saved the camera.yaml output from calibration into the lekiwi_bringup/config/ directory. Then I updated the webcam.launch.py launch file (from the camera_ros package I'm using for the USB webcam) to load the calibration parameters via the camera_info_url parameter.

Now, when the camera node launches, it publishes:

• Raw images on /camera/image_raw

• Compressed images on /camera/image_raw/compressed

• Camera intrinsics on /camera/camera_info

Any downstream node that needs geometrically accurate images can subscribe to both the image and camera_info topics and apply undistortion. For visualization in Foxglove, I used the compressed image stream, which is more bandwidth-efficient.

Foxglove's Image Panel makes it trivially easy to visualize the impact of calibration. The panel has a Rectify toggle that, when off, shows the raw or compressed images straight from the camera, and, when on, applies rectification using the camera_info message.

The difference is pretty subtle on this particular camera. The barrel distortion isn't severe enough to be immediately obvious in casual viewing. However, for applications like visual odometry or fiducial detection, even subtle distortions can throw off measurements by centimeters or more. If you use a fisheye lens, for instance, the rectification makes a dramatic difference.

Visualizing in 3D

While the Image Panel is great for inspecting rectification, Foxglove's 3D Panel takes things a step further - it can render the camera feed directly in 3D space, anchored to the camera's coordinate frame. Since LeKiwi's URDF already defines a fixed joint between the camera and the robot body, the camera image is automatically positioned and oriented relative to the robot, so you can see the camera's view from the correct position and perspective within the full 3D scene.

In the image above, the 3D panel renders the /camera/camera_info topic as a frustum in the scene, showing the camera’s FOV. It then renders the actual camera images, either raw or compressed, on that frustum. This is a surprisingly satisfying sanity check.

In addition to the default perspective projection, the 3D panel offers a couple more ways to render images.

• The plane projection option projects the image on the XY plane. This is quite useful when you have a large FOV, like with fisheye lenses. Here’s an example of the plane projection mode in action:

Try it yourself by changing the render mode of the cameras in this example.

• The depth map option renders a depth image as a pointcloud in 3D space. This does not apply to the LeKiwi, since it uses a monocular camera.

Wrapping up

Camera calibration isn't the most glamorous part of robotics, but it's one of those foundational steps that quietly enable everything else. And throughout this process - from waving a checkerboard at a robot to verifying rectification results - using Foxglove made the whole thing significantly less painful.

With the camera now calibrated and ready for use, the LeKiwi is starting to feel like a real robot. Between my earlier article about integrating the LiDAR and this one, the pieces are coming together. The perception stack is now in place, the URDF is ready, and the ros2_control integration is nearly complete, with some final tuning left. Soon, I will be ready to dive into autonomous navigation with Nav2.

My work on the ROS 2-powered LeKiwi is available on GitHub. It's very much a work in progress - new features will be added and existing ones improved as the project evolves. That said, feedback and contributions are always welcome!

With zero React experience, I dove into a tutorial on Foxglove’s blog. A bit of AI help later, I managed to whip up a button that could toggle between two modes: a toggle switch and a push-button. It changes color with each state change - red for inactive, green for active - and publishes a Boolean message (true for active, false for inactive). Plus, I added a feature to select the topic name from the settings panel.

Getting to this point was no easy feat, and by then, I had lost interest in doing this by myself. So, I handed over some ideas to Claude and let it do its thing. After a few tweaks, I ended up with a sleek, rounded rectangle button with animations that also lets users change state colors and add text labels.

I gave it a whirl over the weekend, and it turned out to be a handy tool. Around the same time, I also realized that the e-stop feature in sts_hardware_interface needed to switch from a subscriber to a ROS 2 service, so I vibe-coded those changes into the custom button too.

But here's the twist: the Boolean publisher button is super useful beyond just an emergency stop. I didn't want to lose that feature, so I made it flexible. Now, users can choose between a Publisher and a Service Call. I now have the following options:

• Output Mode: Pick between a Publisher or a Service call.

  • Publisher: Publish a std_msgs/msg/Bool on state change, switching between true and false.

  • Service Call: Triggers a std_srvs/srv/SetBool service on state change, switching between true and false.

• Topic/Service Name: Select the ROS 2 Topic or Service name based on your Output Mode.

• Button Type: Choose your style - Toggle or Push-button.

  • Toggle: Click once to change the state, click again to revert.

  • Push: Hold to change the state, release to revert.

https://youtu.be/IX-E-L1Qqso

I've been testing it locally on my Foxglove app for a few days, and it's been a game-changer. I mostly use the toggle mode with the Service call to trigger the /emergency_stop service. I've already submitted a PR to add it to Foxglove’s extension registry. Once it's merged, It is now merged and you can grab it directly from the extensions section in the Foxglove app. But if you're eager to try it out locally, check out my Foxglove Extensions GitHub repo. It's just the Button panel for now, but I plan to add more vibe-coded extensions as I go.

What's next? I'm all about vibe-coding these apps, especially when I'm not keen on diving deep into a tech that I won’t use regularly (like React). Creating the button panel was a breeze, and now I'm pumped to tackle more. Up next is a teleop extension. A current community extension mimics teleop_twist_keyboard but lacks features like strafing or holonomic motion, which I need for my LeKiwi platform. I’m confident I can vibe-code something better. I still don't know whether to contribute AI-generated code to someone's repo or create a separate extension myself. Either way, this one's going to be a bit more complex as I need to take my time testing it thoroughly to ensure it's not just AI slop.

The button extension was just a side-quest, built out of necessity. And so will be the teleop extension. So it will be a while before I actually vibe-code, test, review, and publish it. Meanwhile, I’m progressing on my LeKiwi build, more about it soon…

Sure, there are existing implementations out there from other makers, but none are quite aligned with my requirements. I wanted something flexible enough to handle my current omni-wheel setup and the SO-101 arm I'm planning to add, and, of course, all the other projects I plan to make with these motors in the future. So naturally, I built my own.

In this post, I will cover my design and implementation process and the challenges I encountered along the way.

Why ros2_control?

The ros2_control framework provides a plugin-based architecture that cleanly separates hardware from controllers. The idea is simple - write your control logic once, then swap hardware implementations without touching application code. This modular architecture has three main components:

• Hardware Interfaces: Talk directly to motors, handling the low-level communication details

• Controllers: Implement the control algorithms - PID loops, trajectory tracking, omni-wheel kinematics

• Controller Manager: Orchestrates everything, loading and unloading controllers on the fly

Implementation Overview

The hardware interface extends ros2_control's SystemInterface class, which handles lifecycle management and exposes state/command interfaces:

class STSHardwareInterface : public hardware_interface::SystemInterface {
  // Lifecycle: on_init, on_configure, on_activate, on_deactivate, on_cleanup, on_error
  // Core: read(), write()
  // Exports: export_state_interfaces(), export_command_interfaces()
};

Key Design Decisions

Bus-Level Architecture: One Interface to Rule Them All

Most implementations I found were single-motor affairs or locked to specific robot configurations. I needed flexibility - control an entire serial bus with multiple motors, potentially in different operating modes.

Here's the thing about STS servos: they're daisy-chained on one communication line. Each motor has a unique ID, but they all share the same serial port. With my approach, one plugin handles the entire chain. One motor or ten motors in mixed modes - doesn't matter.

Mixed Operating Modes

Right now, LeKiwi's three omni-wheels run in velocity mode. But when I add the SO-101 arm (it's happening, I promise), I'll need position mode for the joints, all on the same serial bus. The solution: configure each joint's operating mode independently.

// Parse operating_mode (per joint)
if (joint.parameters.count("operating_mode")) {
  try {
    operating_modes_[i] = std::stoi(joint.parameters.at("operating_mode"));
  } catch (const std::exception &) {
    RCLCPP_ERROR(logger_, "Joint '%s': Invalid operating_mode value: '%s' (must be 0, 1, or 2)",
      joint.name.c_str(), joint.parameters.at("operating_mode").c_str());
    return hardware_interface::CallbackReturn::ERROR;
  }
} else {
  // Default to velocity mode if not specified
  operating_modes_[i] = MODE_VELOCITY;
}

// Validate operating mode
if (operating_modes_[i] < MODE_SERVO || operating_modes_[i] > MODE_PWM) {
  RCLCPP_ERROR(logger_, "Joint '%s': Invalid operating_mode: %d (must be 0, 1, or 2)",
    joint.name.c_str(), operating_modes_[i]);
  return hardware_interface::CallbackReturn::ERROR;
}

Efficient Batched Communication

The SCServo_Linux SDK handles the low-level serial communication with the motors. My favourite feature is SyncWrite, which batches commands to multiple motors in a single packet instead of sending them one by one.

But why stop there? During initialization, I pre-compute motor groupings by operating mode and pre-allocate all SyncWrite buffers. All hail Claude for identifying this optimization. This now happens once in on_init(), not every control cycle:

// Pre-compute motor groupings by operating mode (static after init)
for (size_t i = 0; i < num_joints; ++i) {
  switch (operating_modes_[i]) {
    case MODE_SERVO:
      servo_motor_indices_.push_back(i);
      servo_sync_ids_.push_back(static_cast<u8>(motor_ids_[i]));
      break;
    case MODE_VELOCITY:
      velocity_motor_indices_.push_back(i);
      velocity_sync_ids_.push_back(static_cast<u8>(motor_ids_[i]));
      break;
    case MODE_PWM:
      pwm_motor_indices_.push_back(i);
      pwm_sync_ids_.push_back(static_cast<u8>(motor_ids_[i]));
      break;
  }
}

// Pre-allocate SyncWrite data buffers (IDs are fixed, data overwritten each cycle)
servo_sync_positions_.resize(servo_motor_indices_.size());
servo_sync_speeds_.resize(servo_motor_indices_.size());
servo_sync_accelerations_.resize(servo_motor_indices_.size());
velocity_sync_velocities_.resize(velocity_motor_indices_.size());
velocity_sync_accelerations_.resize(velocity_motor_indices_.size());
pwm_sync_pwm_values_.resize(pwm_motor_indices_.size());

RCLCPP_INFO(logger_, "Motor groupings: %zu servo, %zu velocity, %zu PWM",
  servo_motor_indices_.size(), velocity_motor_indices_.size(), pwm_motor_indices_.size());

Interfaces and Unit Conversions

STS motors speak their own language (steps, raw PWM values), while ROS 2 expects proper SI units (radians, rad/s) and standardized coordinate frames (according to REP-103). The hardware interface translates between these worlds:

Position (State/Command) Interface

• Motor: 0-4095 steps (one full revolution)

• ROS 2: 0-2π radians

static constexpr double STEPS_PER_REVOLUTION = 4096.0;
static constexpr double STEPS_TO_RAD = (2.0 * M_PI) / STEPS_PER_REVOLUTION;

double raw_position_to_radians(int raw_position) const {
  int inverted_raw = STS_MAX_POSITION - raw_position;  // Position inversion
  return static_cast<double>(inverted_raw) * STEPS_TO_RAD;
}

See that inverted_raw line? That's not optional. The motors increment clockwise while ROS increments counterclockwise. I spent an entire Saturday evening watching the motors spin backwards before the lightbulb moment hit.

Velocity (State/Command) Interface

• STS3215: [-max_velocity_steps, max_velocity_steps] steps/s

• ROS 2: [-max_velocity_steps, max_velocity_steps] * 2π/4096 rad/s

• Note: The max_velocity_steps parameter is configurable per joint and defaults to 3400 steps/s for the STS3215. However, the parameter must be updated for other STS series motors, as they have different max speeds.

Effort/PWM (State/Command) Interface

• Motor: [-1000, 1000] (representing -100% to +100% duty cycle, where the sign represents the direction)

• ROS 2 (command): [-max_effort, max_effort] (max_value defaults to 1.0, i.e. 100% duty cycle)

• ROS 2 (state): [-1.0, 1.0] (normalized measured value, completely independent of max_effort)

• Note: The max_effort parameter is configurable per joint via URDF. It acts as a safety limiter, restricting the command range without scaling. For example, if max_effort = 0.5, commands are limited to [-0.5, 0.5], which maps to PWM [-500, 500].

⚠️ Note: PWM mode is currently untested, either in mock mode or with real hardware. Test thoroughly before using.

Acceleration (Command) Interface

• Motor: [0, 254] (protocol constant, each unit apparently representing 100 steps/s² of acceleration)

• ROS 2: Pass raw values directly - no conversion needed

The hardware interface simply clamps acceleration commands to the valid [0, 254] range. Higher values produce faster acceleration. Set to 0 to disable acceleration limiting (max acceleration).

Additional State Interfaces

The motors also report voltage, current, and temperature. These are straightforward scaling conversions

• Voltage: Raw units × 0.1 = volts (raw 120 = 12.0V)

• Current: Raw units × 0.0065 = amperes (raw 100 = 0.65A)

• Temperature: Direct Celsius value, no conversion needed

The bottom line is that the controllers work in clean SI units (except the PWM and acceleration values, of course) regardless of what chaos is happening at the motor level. The conversions are isolated in the hardware interface where they belong.

ros2_control Lifecycle

ROS 2's managed/lifecycle node system is one of those things that looks quite complex on paper, but just makes sense once you see it work. Instead of cramming all initialization steps into a single massive constructor, you get a clean progression through well-defined states. The ros2_control framework builds on this state machine, mapping each state transition to hardware initialization stages.

Here's the journey from power-on to motion:

• on_init - Here, we parse and validate everything upfront - motor IDs, command interfaces, baud rates, parameter ranges. This is where all the configuration errors are caught, with clear error messages, and not during runtime. The motor groupings and the pre-allocations I mentioned earlier? This happens here as well. All of this before ever touching the hardware.

• on_configure - This is where we talk to the real hardware. The serial port is opened, and each motor is pinged to verify that it is actually alive and responding.

• on_activate - Now, we are ready for motion. Each motor is initialized with its correct operating mode, torques are enabled, and crucially, the position and velocity states are zeroed for clean odometry. Skipping this reset will result in the robot teleporting to a random location, which is not fun to debug. This can be disabled by setting the reset_states_on_activate parameter to false, but this is for very specific use-cases.

• on_deactivate - Here, we gracefully shut down the system. All the motors are stopped using mode-appropriate commands, and their torques are disabled. Now, the wheels/joints can safely be moved by hand.

• on_cleanup - Finally, the serial port is closed, SDK resources are released, and everything is returned to a clean state. The state machine is now ready for another cycle if needed.

• on_error - During emergencies, like communication errors, all motors are stopped, torques are disabled, and the system transitions into an ERROR state, which means that manual intervention is needed. This is only meant for emergencies, and thankfully, I haven’t faced any yet. This also means that this state is currently untested.

The Read-Write Loop

Every control cycle, controller_manager calls the read() and write() functions. This is where the actual motor communication magic happens.

read()

This function queries each motor, converts raw measurements to standard ROS 2 units, and publishes them for monitoring:

for (size_t i = 0; i < motor_ids_.size(); ++i) {
  if (servo_->FeedBack(motor_ids_[i]) != 1) {
    consecutive_read_errors_++;
    // ... error recovery
  }
  hw_state_position_[i] = raw_position_to_radians(servo_->ReadPos(-1));
  hw_state_velocity_[i] = raw_velocity_to_rad_s(servo_->ReadSpeed(-1));
  // ... other states
}

Data flows from motors → controller_manager → controllers → /joint_states topic.

write()

This function applies limits, converts ROS 2 units to the motor’s units, and sends commands to the serial bus. For motors grouped by operating mode, if SyncWrite is enabled, the pre-allocated buffers are used. Here’s an example of the velocity mode motors:

// Update pre-allocated SyncWrite buffers for velocity mode motors
if (!velocity_motor_indices_.empty() && use_sync_write_ && velocity_motor_indices_.size() > 1) {
  for (size_t j = 0; j < velocity_motor_indices_.size(); ++j) {
    size_t idx = velocity_motor_indices_[j];
    double target_velocity = apply_limit(hw_cmd_velocity_[idx],
                                          -velocity_max_[idx],
                                          velocity_max_[idx],
                                          has_velocity_limits_[idx]);
    velocity_sync_velocities_[j] = rad_s_to_raw_velocity(target_velocity);
    velocity_sync_accelerations_[j] = static_cast<u8>(clamp_acceleration(idx));
  }

  // Send batched command to all velocity mode motors in single packet
  servo_->SyncWriteSpe(
    velocity_sync_ids_.data(), velocity_sync_ids_.size(),
    velocity_sync_velocities_.data(), velocity_sync_accelerations_.data());
}

Commands flow from controllers → enforced safety limits → unit conversion → motors.

Emergency Stop

Ever watch a robot go crazy while you frantically search for the power switch? Yeah, me too. That's why I added an emergency stop into the hardware interface as a first-class feature, not an afterthought.

Another amazing feature of the STS protocol - Broadcast - lets you command all motors at once. One packet controls the entire serial bus without looping through motor IDs, which is perfect for an emergency stop. This can be triggered via a ROS 2 service:

# Stop everything NOW
ros2 service call /emergency_stop std_srvs/srv/SetBool "{data: true}"

# Resume normal operation
ros2 service call /emergency_stop std_srvs/srv/SetBool "{data: false}"

The hardware interface creates a ROS 2 service server (during on_configure()) that accepts std_srvs/SetBool requests. Incoming service calls are processed every read() cycle via spin_some(). On receiving a request, the callback sets the flag, and the next write() cycle broadcasts the stop. The service returns success: true and a confirmation message on both activation and release. Just like in the lifecycle states, when the emergency stop is enabled, all motors are stopped, and their torques are disabled. The torque is enabled when the emergency stop is released.

Mock Mode

I’ll be honest, I don't strictly need mock mode for LeKiwi's omni-wheels. The motors are in wheel mode, with no position limits, so testing on real hardware is pretty safe. I’ve also included a raised platform to make sure that the wheels aren’t touching the floor.

But I implemented it anyway, mainly because of the SO-101 arm that's coming, which has servo joints with hard position limits. Send the wrong command, and you're physically damaging the robot. And since ros2_control offers this feature, why not make use of it?

I can now test test the entire control stack with zero hardware at risk. And I’m mocking not only the command interfaces but also the state interfaces - the voltage drops under load, temperature rises with activity, and the current draw matches the effort. This means that when you monitor /dynamic_joint_states, you see believable numbers instead of zeroes.

Is it a perfect simulation? Absolutely not. But at least it is good enough to catch bugs before it ends up breaking hardware. The mocked emergency stop behavior matches the real hardware as well. The mocked voltage, current, and temperature interfaces are just a fun little addition.

You can enable it in the URDF:

<param name="enable_mock_mode">true</param>

Or via launch arguments (recommended):

ros2 launch sts_hardware_interface single_motor.launch.py use_mock:=true

The payoff? Switching between simulation and real hardware is one parameter flip. Zero code changes needed.

Try it out

If you're running STS servos with ROS 2, the sts_hardware_interface handles the hard parts: unit conversions, mode switching, error recovery, mock simulation. Code lives on my GitHub. Give it a shot.

ros2_control and sts_hardware_interface have been powering my LeKiwi mobile base for a month now. Velocity control for the omni-wheels works perfectly. Position mode for the upcoming SO-101 arm is almost there - it needs some testing and possibly tweaks. PWM mode is implemented but hasn’t been tested. It's probably a little over-engineered, and not a perfect implementation - I've prioritized features I actually need - but it works, and it's a solid baseline for current and future projects.

Note that configurations other than the LeKiwi mobile base haven't been tested and won't be for a while. If you give it a shot with different motors or configurations, let me know - feedback is appreciated, contributions even more so.

What’s Next

I’ve already updated the LeKiwi URDF to work with ros2_control and implemented the built-in omni_wheel_drive_controller, which works like a charm out of the box. I’m planning to document it soon, but I promise it won’t be as long as this one, as I didn’t have to write new code. I also need to address some minor tuning issues. This weekend, I intend to calibrate the camera on the LeKiwi, which I must get back to…

PS: Turns out hardback Asterix/Tintin books are perfect backing for a calibration checkerboard printout 😀

Originally written for and published on the Foxglove blog.

A few months ago, I purchased a LeKiwi robot kit with the intention of exploring the LeRobot framework and creating embodied AI applications with it. However, my experience at ROSCon 2025 in Singapore was a turning point, inspiring me to build with ROS once again, so I decided to repurpose the LeKiwi platform for this journey. With my sights set on using Nav2, the navigation stack for ROS 2, I decided to add a 2D LiDAR to the mix. In this article, I will guide you through my workflow and explain how Foxglove was a crucial tool at various stages of my design process.

What's LeKiwi?

Launched in early 2025 by SIGRobotics, a student group from the University of Illinois Urbana-Champaign, LeKiwi is a low-cost, open-source mobile manipulator. Designed to pair seamlessly with LeRobot, HuggingFace's robotics framework for physical/embodied AI, it features an omni-wheeled base, a camera, a Raspberry Pi 5, and mounting points for the popular SO-101 robot arm. The base and arm share the same STS3215 serial bus servo motors, making integration with existing LeRobot setups a breeze.

With the growing popularity of LeRobot, it became an instant hit, especially among people who already had an SO-arm. Several companies also started offering fully assembled and DIY kits with everything you need. I was already on the hunt for a new robot platform, and since I already had experience with the SO-100 and the STS3215 motors, I couldn’t resist. So, I grabbed a kit to see what the buzz was about.

Why add the LiDAR?

After I changed directions and decided to work with ROS 2 and Nav2, which need localization and mapping data for autonomous navigation, adding a LiDAR was a no-brainer. Packages such as slam_toolbox can then be used to create a map using the laser scans, and then localize the robot within this map. First, I set the arm aside temporarily and zeroed in on the mobile base.

With or without ROS 2, adding the LiDAR was a smart move. The LeKiwi base has a front-facing camera, and even with an SO-101 arm with a wrist-mounted camera attached, the rear is still a blind spot. A simple 2D laser scanner offers a budget-friendly way to achieve 360-degree obstacle detection. Even when an arm is attached, and parts of the laser scan are obstructed, the robot's blind spots are still covered.

Integrating the LiDAR

I used my spare LD19 LiDAR and quickly designed and 3D printed a mounting plate. The sensor was now secured, with the USB adapter glued underneath. I then used four 35mm spacers to attach it above the Raspberry Pi. I eventually want to reattach the SO-101 arm, so I made sure to leave the mounting spot for it untouched. Finally, I connected the adapter to the Raspberry Pi with a USB cable.

Next, I tested the setup using the LDLidar ROS 2 package provided by the manufacturer, rather than other available alternatives. It wasn't the best choice, as the package had several bugs. But with AI tools at my disposal, fixing them was a breeze. I have pushed these fixes to my working fork of the LDLidar ROS 2 package.

Finally, it was time to visualize and test the LiDAR using Foxglove. I ran the following commands on separate terminals on the robot's Raspberry Pi.

ros2 launch ldlidar_ros2 ld19.launch.py

ros2 launch foxglove_bridge foxglove_bridge_launch.xml

On my laptop, which is on the same network as the Raspberry Pi, I simply launched the Foxglove app, connected to the Foxglove Bridge on port 8765, and the laser scan visualization appeared on the 3D panel right away.

https://www.youtube.com/watch?v=QbdykAOCLRo

URDF and ROS 2 Control

Aided by AI, I set off to create my own LeKiwi ROS 2 package, following these steps:

First, I tackled the robot description file (URDF). It defines the robot's links and joints and maps them to the correct mesh files for visualization. The URDF also allows for adding ros2_control tags to specify command and state interfaces, along with actuator limits. Since the LeKiwi repository contains an older version of the URDF, I created a new one for my robot and made it public in the lekiwi_description package.

Foxglove was quite helpful in visualizing the URDF. The 3D panel provides an option to add a custom URDF layer, which requires a link to a URDF file (URL, ROS 2 parameter/topic, or local path).

I used the URL to the raw URDF on my GitHub repository, which you can try out for yourself without any local ROS 2 installation.

As shown in the image, I’ve also included the links and meshes for the LiDAR and the Camera, which I plan to implement later. Note that the warning signs in the above images are due to my visualizing a static URDF from a URL and not having a live connection to the robot at the time of testing.

With the robot's links, joints, and meshes visualized correctly, the next step was to control the motors and move the robot. For this, I used ros2_control. I created a hardware interface for the STS3215 motors and then configured the omni_wheel_drive_controller, which is provided in ros2_control.

You can find my implementation of the hardware interface and control configuration in the sts_hardware_interface and lekiwi_control packages, respectively. Note that the hardware interface was extensively tested only with the STS3215 motors. Please proceed with caution when using other STS series motors or untested modes (such as servo or PWM mode).

I tested everything using Foxglove, making adjustments to the control configuration and URDF along the way. On the Raspberry Pi, I used three separate terminals to run the controller, the Foxglove bridge, and a TwistStamped publisher (the controller requires twist messages with a timestamp) using the following commands:

ros2 launch lekiwi_control control.launch.py

ros2 launch foxglove_bridge foxglove_bridge_launch.xml

ros2 topic pub -r 10 /base_controller/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link'}, twist: {linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"

And here are the results:

https://www.youtube.com/watch?v=ZMS3ILDSjJs

In this video, the robot is placed on a platform, so the motors are moving, but the wheels are not touching the floor. This means that the odometry, computed from the measured velocities of each motor, is quite accurate. With this test, I validated the URDF and the default control setup before integrating with the LiDAR.

Integration and Testing

To integrate the control package with the LiDAR, I set up a combined launch file in lekiwi_bringup. This launch file starts both the ros2_control and LiDAR nodes with a single command. I also included teleop_twist_joy in the launch file to convert joystick inputs into TwistStamped messages for the control node.

For the joystick inputs, I used the standard joy package to read data from a game controller and format it into a Joy message. I wanted to control the robot from my laptop, so I connected my game controller to it and ran the joy node there.

ros2 run joy joy_node

For communication between my laptop and the Raspberry Pi, I was already using Tailscale and decided to work with ROS 2 Kilted and Zenoh RMW as the middleware. However, this can be done in several different ways. For example, you could connect both devices to the same Wi-Fi network and use the different configurations provided by DDS alternatives like FastDDS or CycloneDDS. The integrated system looks like this:

Finally, I fired up the Foxglove Bridge and used the Foxglove app to visualize the robot in action as I navigated it with my controller. On the Foxglove app, I used the Plot panels to compare the measured velocities from the Odometry messages with the command velocities from the teleop_twist_joy node. This helped me determine realistic limits for the twist commands. Plus, the Diagnostics panels were really handy for keeping tabs on the ros2_control diagnostics data. I’m definitely using these tools for fine-tuning the robot's odometry (which is surprisingly accurate right out of the box, just a bit noisy) and for my adventures with Nav2.

Summary

With the addition of a LiDAR and the ros2_control implementation tested using a game controller, I'm well on my way to modifying LeKiwi into a ROS 2-powered autonomous mobile robot. Foxglove has been an invaluable tool throughout this journey, from creating the URDF to fine-tuning teleoperation and testing the integrated system, and all on a MacBook (which famously has limited support for native ROS 2 installations). While the foundation for this project seems solid, there’s still work to be done before starting with autonomy.

For starters, the ros2_control configuration works with the default settings, but it needs to be fine-tuned. This includes the odometry, which has been surprisingly accurate, but it is too noisy and could use a filter. I also want to calibrate the camera and work with ROS 2 camera packages.

I am sure that Foxglove will continue being a key utility for what’s to come. Looking ahead, I’m excited to see this robot navigate around autonomously, even though I do not have a specific application for it. But I am certainly learning a lot during this journey and can’t wait to see where it leads!

What’s FOSDEM?

If FOSDEM sounds unfamiliar, you're not alone. I only discovered it when the Robotics and Simulation devroom debuted in 2025. After hearing rave reviews, I decided to check it out this year. FOSDEM is a free annual event at the ULB Solbosch campus celebrating all things free and open source. With thousands of talks and several devrooms, each focusing on specific topics, it's a techie's paradise.

I initially planned to stick to the Robotics and Simulation devroom, but it was way more popular than anyone expected. Leaving for lunch, which I did, meant a long wait in line to get back in. With limited time, I opted to explore other FOSDEM areas and wander around Brussels instead of queuing up.

If you know me, I am more interested in hardware than software, so I was naturally attracted to the stands showcasing open-source hardware projects. Here are a few interesting projects I came across:

OpenFlexure Microscope

An open-source, 3D printable, Raspberry Pi-powered microscope. Not a hobby/education tool like the ESPressoscope, but real research equipment actually deployed in the medical and research sector in 70+ countries. One of the creators, present at the stand, told me about how they deployed a few in rural Rwanda, where more expensive tooling is not accessible. And looking at the live images from their demo microscopes, it looked really high quality, accurate, and in focus. All from a Raspberry Pi HQ camera, and off-the-shelf stepper motors with PLA parts for positioning. Very cool.

Open Micro-Manipulator

At the FreeCAD and KiCAD stand, they demonstrated products created using these open-source CAD and PCB design tools, one of them being the Open Micro-Manipulator. This is an XYZ-manipulator and motion control platform - made using NEMA stepper motors, 3D printed parts, and commercially available hardware components - achieving sub-micron precision. This was another project that blew my mind. To top that, the entire control system is powered by a Raspberry Pi Pico. I really need to dig into this project when I get the chance. I have the parts to build one, maybe I’ll make a weekend project out of it someday.

OpenPrinting

They provide open-source drivers for paper printers, like a Klipper for 2D printing. Unfortunately, there was nobody at the stand when I was there; they had probably gone for lunch. But if I ever decide to get a paper printer, I’m definitely getting one that’s compatible with OpenPrinting.

Robotics and Simulation Devroom

I reached the venue about 15 minutes before the day started, and was lucky enough to get a choice of seats. I said hi to some people, and after trying out a few spots, found the perfect place to sit. But soon, as more people started coming in, the room was full.

I left to get a coffee and some food, and that’s when I realized how crazy it was outside - there were about 20 people in line, waiting to get in. By the time I returned, the line had gotten even bigger. I decided to first explore other areas of FOSDem and then watch the livestreams for the rest of the talks. I’ve clubbed the interesting ones into two main categories - Robots and Rust. There were a few other memorable ones, which I have another section for below.

Robots

HackerBot

First, Stef Dillo, the creator of the Tenacity Rover, talked about another project of his - converting a cheap robot vacuum into a ROS-powered robot base, with a hacked mapping solution that converts the vacuum’s internal map into a ROS-compatible costmap. He talked about his challenges, how he fixed them, and shared some general tips and tricks.

https://www.youtube.com/watch?v=ykVGcZc0c0A

It is a shame he didn’t share the actual make and model of the robot vacuum, but he did mention HackerBot Industries, a company that sells complete kits (but also does not mention the original manufacturer). This is something I’ve been wanting for years - a hackable robot vacuum cleaner. There’s the iRobot Create platform, but it does not include any vacuuming features. I guess I will have to build my own.

Just1

In a lightning talk, Nicolas Rodriguez showed (and demoed live) his mecanum-wheeled robot called Just1. It reminded me of my time working on AKROS, but this one was much more affordable. Just1 costs only 250 Euros to build, and this includes a 70 Euro estimate for the LD19 LiDAR, which these days costs 20-30 Euros on Chinese websites. Perfect solution for students/hobbyists. Combine the Just1 with Linorobot, and you’ve got a powerful robot on your hands, all for about 200 Euros.

https://www.youtube.com/watch?v=m_tZ3eg-KsA

iXi

Botronics, a Belgian startup, did an extensive deep-dive into the technology stack of iXi, their autonomous golf-bag carrying robot. I was definitely impressed by their commitment to open-source, and they’re probably the only company I know of that is building a product without re-inventing the wheel. They use off-the-shelf parts, existing ROS 2 packages where possible, and build on top of that, contributing back to the community. And the things they’ve built on top are quite impressive as well. Do check out their talk to learn more.

https://www.youtube.com/watch?v=Wpu0oe_uBDM

RustyRover

In the second lightning talk session, engineers from Botronics showcased RustyRover, a DIY differential-drive robot built with Rust. They built it to experiment with Rust and assess new technologies. Botronics seems to have cultivated a really cool company culture, and it is very impressive. And a cool project as well, I’m definitely going to read through this as I slowly get started with Rust.

Rust

Copper-rs

Guillaume Binet talked about his project, Copper-rs, a Rust-based, game-engine-like, deterministic framework for robotics. Very cool project and impressive results, but I am a bit too biased towards ROS, and while I do want to learn Rust, I would rather use it with ROS 2 rather than a completely new framework. But it was good to learn about Copper-rs, I’ll read more when I get the chance.

rclrs / ros2_rust

Esteve Fernandez talked about rclrs - the ROS 2 client library for Rust. I’ll be honest, I was in line at a food truck at the time, and I completely missed this. But I was at Esteve’s talk at ROSCon last year, so I had an idea of what he spoke about. I’m adding it to this post because this is one of the projects I am curious about, and I plan to try it out as a part of my Rust learning journey

ROS-Z

This was another project I had heard about at ROSCon, but I believe it was a lightning talk. This was a much longer presentation about it. ROS-Z is simply ROS 2, but with parts of the ROS 2 stack replaced by pure Rust-based components, and using Zenoh as the middleware. So you could have a Rust-native stack, but still have your C++/Python apps. I wasn’t that interested when I first learned about it at ROSCon, but after using Zenoh, I don’t think I will move back to DDS again. Now it makes sense to move to ROS-Z.

I am a bit conflicted between working with ROS-Z or ros2_rust as a potential side-project. But I have some time before I pick a lane. But at least after these talks, I am even more motivated to learn Rust. Now to find the free time…

Memorable talks

Besides this, there were a few other memorable talks. Sam Pfeiffer and Roland Meertens talked about getting good-quality robot data from different types of sensors, and shared their personal experiences along with some tips and tricks. There was also a talk about EasyNav, a lightweight navigation stack with some architectural decisions that make it quite different from Nav2. Finally, Davide Faconti talked about the future roadmap of PlotJuggler. I watched the live-stream while on the bus back, and laughed so hard that I could have been kicked off the bus. I’m not going to say anything else; watch this talk for yourselves.

https://www.youtube.com/watch?v=cy--EC-YKCQ

Summary

Overall, my first FOSDEM was a bit of a hectic experience, and a little overwhelming, but in a positive way. But now I can prepare for next year, and I hope they have a bigger space for the Robotics devroom at FOSDEM 2027. I’m definitely coming back, and given the projects I am working on this year, I have things I would like to present as well.

Speaking of presenting, the talk from Botronics included a surprise at the end - the announcement of ROSCon Belgium in 2026. The spoken language is English, which means I’m definitely attending and submitting a talk when the time comes.

This image is from the Botronics talk. I really like this logo, especially the Moon Rocket from Tintin; I hope they confirm it. I also managed to visit a comic book store in Brussels on my way back - got myself the newest Asterix book, and a couple of Tintin comics in Dutch (I’m getting better at Dutch, so I’m going to give this a shot). Turns out, while it is Tintin in both English and French, the Dutch character is called Kuifje.

What’s next?

Thanks for FOSDEM, I have even more project ideas now. But this year, I want to focus on only a handful of projects, especially after registering a business - I want to continue doing projects with the LeKiwi robot, I’ve got a long-term product idea I’m working on (more on this soon), and I want to learn Rust. Luckily, the talks were quite relatable to the things I’m working on, and I’m glad. Now, back to working on LeKiwi’s ros2_control implementation…