Imandra interface to Robot OS: Part I

At AI, we’ve been working on an IML (Imandra Modelling Language) interface to ROS, allowing one to develop ROS nodes and use Imandra to verify their properties. This post is the first of 2-part sequence where we demonstrate how to create and verify a simple ROS node model in Imandra. In this post, we’ll focus on the creation of the ROS node model using IML/OCaml and will run it as a part of the ROS system.

We’ll create a controller that uses the laser scanner to avoid obstacles and drive around the scene. The Imandra ML code can be compiled in OCaml and plugged into the ROS system — the behaviour of the bot can be observed in the Gazebo simulator.

Then we’ll illustrate how to use Imandra to formally verify various statements about the model and how to find bugs and corner cases by exploring the Imandra-generated counterexamples for false conjectures.

ROS message OCaml types

For our Imandra-ROS project, we’ve processed all of the standard ROS messages with our code generation tool, creating a collection of strongly-typed IML/OCaml bindings for them.

The Twist messages from the geometry_msgs standard ROS package are used to control the robot. We want to mimic ROS messaging nomenclature as closely as possible, so we define a module with the same name as the package, and we’ve placed the necessary type/message declarations inside:

You might have noticed that we’ve replaced floating point values for vector coordinates with integers. In this context, it is more straight-forward for Imandra to reason about integers, so we assume that there is a common factor of 100,000 multiplying all the incoming floating point values and divides all the outgoing integers. (That effectively makes our unit of measurement of length to be 10 micrometres).

Moving on, we declare the incoming messages:

• LaserScan sensor input message from the sensor_msgs ROS package
• Clock message from the Rosgraph_msg ROS package

We define the wrapping modules for both messages and declare their datatypes:

Robot OS middleware will communicate with our node via messages of these three types. The code that we’ll write for of our node will represent the formal mathematical model of the controller algorithm — we can then use Imandra to reason and verify various statements about the code. Since IML is valid OCaml, we’ll also leverage its compiler to create a high-performance executable from the verified IML code.

Creating a simple ROS Node model

We want to create a simple but non-trivial robot controller that safely makes our bot drive around avoiding obstacles. The bot is going to drive forward until one of the laser scanner ranges becomes too low, meaning that we’ve gotten too close to some obstacle — in that case, we want the bot to stop and turn until the scanner tells us that the road ahead is clear. To make the model a bit more complicated, we’d like to implement the ability to choose the turning direction depending on the laser scanner ranges.

One might try to make a “naive” controller that doesn’t have any memory about its previous states and observations — such a bot would react to the currently observed scanner values and decides its actions based solely on that information. This approach would quickly lead to the bot being “stuck” in infinite oscillatory loops. E.g. here is a bot that decides which side to turn depending on the first value in the ranges array:

To avoid these kinds of oscillations, we need the model to have some memory of its previous states. Our idea is to introduce two modes of the model’s operation: driving forward and turning in one place. The bot is in the “driving” mode by default, but it can transition to the turning mode if it gets dangerously close to surrounding objects.

The turning direction is calculated using the direction of the minimum of the distances that the scanner returns. While the robot is turning in one place, it stores the minimal range that the scanner has detected at that location. If at some point the scanner detects a range that is lower than the stored one, then the turning direction gets recalculated, and the minimal range gets updated.

State datatype

Working with Imandra we’ve adopted a standard way to construct formal models of message-driven systems. At the top of the model we have a single OCaml datatype that holds all the data needed to describe the system at a given moment, including incoming and outgoing messages. We call this record type state. Together with this state type we define a one_step transition state->state function, which performs a single logically isolated step of the simulation and returns the new state after the transition.

As an example, consider an IML/OCaml type declaration for a simple ROS node that can accept rosgraph_msgs/Clock and sensor_msgs/LaserScan standard ROS messages. We also want the state to store three values:

• the current mode of the bot — whether we are driving forward or turning in a preferred direction
• the latest minimal value of the ranges that the laser sensor returns
• the preferred direction for the robot to turn — either clockwise CW or counter-clockwise CCW

Finally, we want the node to be able to send geometry_msgs/Twist ROS message depending on the stored min_range data:

State transition one_step function

To implement our node, we’ll need a function that scans through a list of values and returns the minimum value and its index. We’ll make a generic function foldi that does an indexed version of the List.fold_right standard OCaml function:

When accepting this function, Imandra constructs its “termination proof” — that means that Imandra managed to prove that recursive calls in this function will not end up in an infinite loop. Imandra proves such things using inductive reasoning and is able to prove further statements about other properties of such functions.

On an incoming Clock tick we are simply sending out a Twist message which tells the robot to either move forward or turn, depending on the mode that it is currently in. We encode it by introducing the make_twist_message helper function and the process_clock_message : state -> state function.

On incoming Scan message, we want to find the minimum of the received ranges and the index of that minimum in the list. Depending on the index, we decide in which direction to turn. To implement this, we create another helper function and the process_sensor_message one:

With the help of these functions, we can create our one_step transition function, which just dispatches the messages to the appropriate helper function above.

Running the model as a ROS node

Now that we have an early model, let’s compile it with our ROS node wrapper into an executable. Here is the model, controlling our “imandrabot” in the Gazebo simulation environment:

That concludes the first part of our series on designing and verifying ROS nodes with Imandra. In this post, we’ve created a formal model of the ROS node controller for a 2-wheeled bot. In the next post, we’ll look at how one can use the Imandra reasoning engine to formally verify various statements about the ROS node and explore edge cases that one discovers when constructing the formal proofs about the model.

To learn more about our Imandra-ROS project, please visit our GitHub. To stay in touch with our updates, please sign up on our website.