Wishful Coding

When I said that ROS would be a steep learning curve, I did not expect to become a Docker expert and a kernel developer in the progress. But in the end it worked, with only a few seconds of lag.

Meanwhile RoboTeam Twente won a game against Robodragons, but that’s sadly the only game they won. The finals looked a bit more fast-paced than this game, but not by a lot. It’s 10 minutes game time in an hour clock time. These top teams have impressive ball control though. There is some work to be done for Twente… But for now, back to ROS and EV3.

Installing ROS on the EV3

I headed over the ROS installation page, which states that on Debian they only provide x64 packages, not ARM. So I was kindly redirected to their page about compiling from source. However, the EV3 is far from powerful enough to compile ROS on the device, so a cross-compiler is needed.

Ev3dev uses Docker images with Qemu to do this. But they provide several kinds of images, useful for either cross-compilation or for generating boot images for the EV3. However, ROS does not have a nicely self-contained installation process, so what I needed is a cross-compilation image that could also generate a boot image.

ROS is like a fractal of package management, breaking at every stage. First you install Python packages to install stuff in /etc and then you install a bunch of stuff into a workspace, and then you tell it to install system packages for the dependencies of that stuff. Then you combine this into a standalone workspace, which can be used install more packages in more workspaces.

So at this point I was learning about Dockerfiles and base images and Qemu and ROS releases and contexts and environments and source lists. And after a day of trying and hours of waiting, I had a shiny boot image with my cross-compiled ROS installation. And then David Lechner casually mentioned Debian does actually provide ARM packages, available with a simple apt-get install ros-robot.

Learning ROS

So after you’re three levels deep in workspaces that you have sourced, you can begin creating your own packages, by – you guessed it – more package management. So you edit your package XML file for the dependencies of your package and run rosmake which is like CMake for ROS. You also need to run rosmake on Python projects to resolve dependencies and generate more code.

In exchange for all this work, ROS provides a lot of powerful tools, like roscd, which is like cd but for ROS, rosed, which is like vim but for ROS, and rosrun which is like running your code, but for ROS. I think you can guess what rosls is for.

After you have created a package, you can start creating nodes. Nodes can be written in a number of languages, as long as they can talk to the central ROS server which routes all the messages on different topics to all the interested nodes. As per Greenspun’s tenth rule, ROS contains an ad-hoc, informally-specified, bug-ridden, slow implementation of half of Erlang. An actor system is not provided, only callbacks.

So now you have all these packages and nodes, and of course it would be silly to rosrun all of them individually. So they provide roslaunch, which is like init but for ROS. So you write some more XML, and roslaunch will do the rest. It will even ssh into remote machines to launch nodes on it.

The goal was to run ROS on the EV3, but not all of ROS. To be more specific, one single node. The EV3 does not have enough RAM for two operating systems at the same time. So on the EV3 you source all the workspaces and roscd into your package where you can rosrun the node. But before doing that, you have to tell it where the server lives with export ROS_MASTER_URI=http://pepijn-Latitude-E6420.local:11311. I tried to get roslaunch to do it, but it’d run out of RAM and crash.

So next I ran the listener from the tutorial on the EV3 and the talker on my laptop. Everything would appear to be fine, but no messages arrived. Long story short, it turns out you also need to set ROS_HOSTNAME or ROS_IP on both ends so that they can actually reach each other.

Writing a ROS node

With ROS running on my laptop and the EV3, and the basic listener working, it was time to actually write some code. But first, some more package management!

ROS has a joy package, to read joysticks, and a teleop_twist_joy package that provides a node that converts joy to twist messages. These packages are supposed to be in your system package manager, but they are not. So I installed them from source, but… package management… fast forward… I apt-get purge the Ubuntu packages and use the official ROS Melodic packages.

<launch>

  <node pkg="joy" name="joy" type="joy_node" output="screen">
    <param name="dev" type="str" value="/dev/input/js0" />
    <param name="deadzone" type="double" value="0.1" />
  </node>

  <node pkg="teleop_twist_joy" name="twist" type="teleop_node" output="screen">
    <param name="enable_button" type="int" value="4" />
    <param name="axis_linear/x" type="int" value="0" />
    <param name="axis_linear/y" type="int" value="1" />
    <param name="scale_linear/x" type="double" value="1" />
    <param name="scale_linear/y" type="double" value="1" />
    <param name="axis_angular" type="int" value="3" />
    <param name="scale_angular" type="double" value="1" />
  </node>

</launch>

Now I can finally start writing code on the EV3. So I copy the Python code from last time, and replace the joystick code with rospy listener code. As I get ready to test my code, it dawns on my that rospy is Python 2, while ev3dev-lang-python is Python 3. I go back to my Docker image and change it to Python 3. An hour later it crashes with an Unicode error, and another hour later with a package version mismatch. The next day it finally works, but it seems to be missing half the packages.

I consider writing my node in C++, but worry about all the package management needed to get ev3dev-lang-c++ into the ROS universe. I consider my alternatives, and without further package management, I succeed in using a Vala/GTK/GObject binding called Ev3devKit from Python 2.

import gi
gi.require_version('Ev3devKit', '0.5')
from gi.repository import Ev3devKit

manager = Ev3devKit.DevicesDeviceManager()
motors = {m.get_address(): m for m in manager.get_tacho_motors()}

def run(port, speed):
    m = motors['ev3-ports:'+port]
    if speed==0:
        m.send_command('stop')
    else:
        m.set_speed_sp(speed)
        m.send_command('run-forever')

With that in place, all that is left is a few trivial changes to the code from last time.

#!/usr/bin/env python2

import threading
import numpy as np
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
import rospy
import kit

## Initializing ##

angles = np.deg2rad([-36.8, -90, 36.8])
coef = np.array([np.sin(angles), np.cos(angles), [-1,1,1]]).T

speed = np.zeros(3)
kick = 0
running = True

class MotorThread(threading.Thread):
    def run(self):
        print("Engine running!")
        while running:
            sp = coef.dot(speed)
            kit.run('outA', sp[0])
            kit.run('outB', sp[1])
            kit.run('outD', sp[2])
            kit.run('outC', kick)

        kit.run('outA', 0)
        kit.run('outB', 0)
        kit.run('outD', 0)
        kit.run('outC', 0)

motor_thread = MotorThread()
motor_thread.start()

def callback(data):
    #rospy.loginfo(data)
    speed[0] = data.linear.x*-300
    speed[1] = data.linear.y*300
    speed[2] = data.angular.z*100

def btn_callback(data):
    global kick, running
    btn = data.buttons
    if btn[1]:
        kick = 1000
    elif btn[3]:
        kick = -1000
    else:
        kick = 0
    
    if btn[2]:
        running = False
        rospy.signal_shutdown('button pressed')
    
def listener():
    rospy.init_node('ev3dev', anonymous=True)
    rospy.Subscriber("cmd_vel", Twist, callback)
    rospy.Subscriber("joy", Joy, btn_callback)
    print("subscribed")
    rospy.spin()

if __name__ == '__main__':
    listener()

This is a story about how my curiosity led me to be conscripted into a student team.

One of my friends is a member of RoboTeam Twente and is currently in Canada with the rest of the team to compete in the RoboCup Small Size League. They only started last year, and will be competing against teams that have been playing for decades. You can follow their progress live here

Having worked non-stop on my bachelor thesis in Electrical Engineering, my hands were itching to do some programming. So I nudged my friend if I could maybe entertain myself with their code. It turns out a few weeks before the competition is not a good time to introduce new people to the codebase, but this initiated an avalanche of requests to please join the team of next year, full-time please, and we’re looking for board members.

However, this left me with my immediate itch to program something. Since I already started thinking about ideas for their soccer robots, I figured I might as well build my own robot from LEGO to satisfy my itch, while they are having fun in Canada. Before I knew it, I had already ordered a set of omni-wheels and borrowed an orange golf ball from the team (on the condition that I’d join… oh well)

The RoboTeam robot is a holonomic platform with 4 omni-wheels that allow it to move in any direction. It has two solenoids for kicking and “chipping” (kicking the ball in the air) powered by a 200V capacitor. It also has a “dribbler”, which is a rotating bar that keeps the ball against the kicker while moving.

My first challenge was figuring out how to implement that with an EV3 with only 4 motor ports. I figured I could do with 3 omni-wheel, and ditch the chipper. That leaves one motor for both dribbling and shooting. I thought that maybe with some slip gears I could dribble going one way, and shoot going the other way. The most powerful shooting technique I could think of is to compress a spring with a worm wheel, and release it.

I went through several iterations of this crucial and complex part, trying to make it more compact and sturdy. I used a medium motor that connects to a worm wheel and to a normal gear that drives a perpendicular axle. The worm gear drives a slip gear to a crankshaft that pulls the kicker back and releases it. The perpendicular axle drives another slip gear that drives the dribble bar.

After being somewhat satisfied with the kicker/dribbler, I moved on to the rear wheel. This was fairly straightforward, making a sturdy housing for the omni-wheel. The only issue was finding a space for the motor. This ended up looking a bit tacky because I had to move the motor to the back because it got in the way of the side wheels.

It was surprisingly hard to design the side wheels. To obtain a proper holonomic platform, most people opt to construct an equilateral triangle, but this was not an option with the kicker in the mix. Instead I went for two 3:4:5 Pythagorean triangles on the sides of the body, giving me 36.8° angles for the side wheels. Note that the side wheel is attached one unit higher than the rear wheel, because the arms attach one unit below the main frame.

Due to the crankshaft of the kicker, I had to place the side wheels quite far back. That in turn meant that the rear wheel motor had to move. This combined means that the robot is larger than would be legal in the SSL. I believe it would be possible to comply with the rules, but a complete redesign is needed. All I wanted is to have some fun and write some software, so I’ll leave it at this.

Now I can finally begin to write software. My initial plan was to use Lejos, as it has ready-made classes for holonomic robots. But after some struggling with a two year old “beta” release, I decided it’d be easier to just do the math in Python myself, and use the much more actively maintained ev3dev-lang-python.

As a start, I blatantly copied this script, and adapted it for my Xbox controller and robot. The math ended up only being a few lines of numpy, much easier than I expected.

Next thing I want to try is to use ROS, which is also used at the RoboTeam. There is some documentation about using it on the EV3, but not much. So that will be a steep learning curve.

#!/usr/bin/env python3

import evdev
import ev3dev.auto as ev3
import threading
import numpy as np

## Initializing ##
print("Finding xbox controller...")
devices = [evdev.InputDevice(fn) for fn in evdev.list_devices()]
for device in devices:
    if device.name == 'Microsoft X-Box 360 pad':
        gamepad = device

angles = np.deg2rad([-36.8, -90, 36.8])
coef = np.array([np.sin(angles), np.cos(angles), [-1,1,1]]).T

speed = np.zeros(3)
kick = 0
running = True

class MotorThread(threading.Thread):
    def __init__(self):
        self.motor_left = ev3.LargeMotor(ev3.OUTPUT_A)
        self.motor_back = ev3.LargeMotor(ev3.OUTPUT_B)
        self.motor_right = ev3.LargeMotor(ev3.OUTPUT_D)
        self.motor_kick = ev3.MediumMotor(ev3.OUTPUT_C)
        threading.Thread.__init__(self)

    def run(self):
        print("Engine running!")
        while running:
            sp = coef.dot(speed)
            try:
                self.motor_left.run_forever(speed_sp=sp[0])
                self.motor_back.run_forever(speed_sp=sp[1])
                self.motor_right.run_forever(speed_sp=sp[2])
                self.motor_kick.run_direct(duty_cycle_sp=kick)
            except OSError:
                pass

        self.motor_left.stop()
        self.motor_back.stop()
        self.motor_right.stop()

motor_thread = MotorThread()
motor_thread.start()

for event in gamepad.read_loop():
    if event.type == 3:
        axis = evdev.ecodes.ABS[event.code]
        if axis == 'ABS_X':
            speed[0] = event.value/(2**15)*300
        if axis == 'ABS_Y':
            speed[1] = -event.value/(2**15)*300
        if axis == 'ABS_RX':
            speed[2] = event.value/(2**15)*50
        if axis == 'ABS_Z':
            kick = (event.value>10)*100
        if axis == 'ABS_RZ':
            kick = -(event.value>10)*100
    elif event.type == 1 and event.code == 307 and event.value == 1:
        running = False
        break

If you define a variable, you use code to generate that data. This is the same concept that underlies atoms in Clojure: Your change is expressed as a function that can be repeated until it succeeds. So what if you maintain a relation between the data and the code that generated it?

This is kind of what happens in Haskell, with its lazy evaluation. You don’t really know if the thing you’re using is evaluated. But you do know it’s only evaluated once, and kept around for as long as referenced.

What if you embrace functional purity, and allow deallocation and repeated evaluation? Below is a silly Clolure function that implements a future that may throw away and recompute your result. Variations of this theme are possible that compute in the current thread for less overhead, or additionally wrap the future in java.lang.ThreadLocal to avoid thrashing the cache. (Seems similar to durable-ref in a way)

(defn soft-future-call [f]
  (let [sref (atom (java.lang.ref.SoftReference. (future-call f)))]
    (reify clojure.lang.IDeref
      (deref [this]
        (if-some [fut (.get @sref)]
          (deref fut)
          (do
            (reset! sref (java.lang.ref.SoftReference. (future-call f)))
            (deref this)))))))

I can’t remember where I read it, but it is said that for single-core machines the fastest code reuses as much data as possible, while on multi-core machines it is often faster to avoid sharing data, and recompute it locally. It may also be interesting for larger-than-memory problems. For example, compiling a huge codebase can use a lot or RAM. Maybe it turns out that recomputing parts of the data is faster than relying on swap space. So it’s maybe an interesting paradigm to write code that abstracts away the distinction between a function and its result.

Well, it’s just a rough idea. Maybe it turns out to be really mundane, boring, and annoying. Maybe it turns out to be really powerful, with the right tools and abstractions.