Creating and Verifying a ROS Node¶
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. In this notebook, we will go through creation and verification of a Robotic Operating System (ROS) node in Imandra. We will make a robot control node that controls the motion of a simple 2-wheeler bot:
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.
1. ROS message OCaml types¶
For our Imandra-ROS project we’ve processed all the standard ROS messages with our code generation tool creating a collection of strongly-typed IML/OCaml bindings for them. But, in order to keep this notebook self-contained we'll define the necessary messaging modules here.
First, we'll need to declare the message type that will control our robot. This is typically done with a Twist
message from the geometry_msgs
standard ROS package. We want to mimic ROS messaging nomenclauture as close as possible, so we'll create an OCaml/Imadra module
with the same name as the package and will place the necessary type/message declaraions inside:
module Geometry_msgs = struct
type vector3 =
{ vector3_x : int
; vector3_y : int
; vector3_z : int
}
type twist =
{ twist_linear : vector3
; twist_angular : vector3
}
end
module Geometry_msgs : sig type vector3 = { vector3_x : Z.t; vector3_y : Z.t; vector3_z : Z.t; } type twist = { twist_linear : vector3; twist_angular : vector3; } end
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).
Let's move on and declare the incoming messages:
LaserScan
sensor input message from thesensor_msgs
ROS package- and the
Clock
message from theRosgraph_msg
ROS package
We define the wrapping modules for both messages and declare their datatypes:
module Sensor_msgs = struct
type laserScan =
{ laserScan_range_min : int
; laserScan_range_max : int
; laserScan_ranges : int list
}
end
module Rosgraph_msgs = struct
type time =
{ seconds : int
; nanoseconds : int
}
type clock = { clock : time }
end
module Sensor_msgs : sig type laserScan = { laserScan_range_min : Z.t; laserScan_range_max : Z.t; laserScan_ranges : Z.t list; } end module Rosgraph_msgs : sig type time = { seconds : Z.t; nanoseconds : Z.t; } type clock = { clock : time; } end
Robotics 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 use Imandra to reason and verify various statements about the code. Since IML is valid OCaml, we'll leverage its compiler to create an executable from the verified IML code.
2. Creating a simple ROS Node model¶
We want to create some simple but non-trivial robot controller that makes our bot drive around avoiding the 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 reacts to the currently observed scanner values and decides its actions based solely on that information. Such an approach will 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 this kind of oscillations we need the model to have some memory of its previous states. The idea is to introduce two modes of 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 - the turning direction gets recalculated, and the minimal range gets updated.
2.1 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 is able to 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 side 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:
type incoming_msg =
| Clock of Rosgraph_msgs.clock
| Sensor of Sensor_msgs.laserScan
type outgoing_msg =
| Twist of Geometry_msgs.twist
type direction = CW | CCW
type mode = Driving | Turning
type state =
{ mode : mode
; min_range : int option
; direction : direction option
; incoming : incoming_msg option
; outgoing : outgoing_msg option
}
type incoming_msg = Clock of Rosgraph_msgs.clock | Sensor of Sensor_msgs.laserScan type outgoing_msg = Twist of Geometry_msgs.twist type direction = CW | CCW type mode = Driving | Turning type state = { mode : mode; min_range : Z.t option; direction : direction option; incoming : incoming_msg option; outgoing : outgoing_msg option; }
2.2 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
:
let rec foldi ~base ?(i=0) f l =
match l with
| [] -> base
| x :: tail -> f i x ( foldi f ~base ~i:(i+1) tail )
val foldi : base:'a -> ?i:Z.t -> (Z.t -> 'b -> 'a -> 'a) -> 'b list -> 'a = <fun>
Termination proof
original: | foldi base ( *opt* ) f_2 l | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
sub: | foldi base (Some ((if Is_a(None, ( *opt* )) then 0 else Option.get ( *opt* )) + 1)) f_2 (List.tl l) | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
original ordinal: | Ordinal.Int (_cnt l) | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
sub ordinal: | Ordinal.Int (_cnt (List.tl l)) | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
path: | [l <> []] | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
proof: | detailed proof
Expand
|
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.
let get_min_range max lst =
List.fold_right ~base:max
(fun x a -> if x < a then x else a) lst
val get_min_range : Z.t -> Z.t list -> Z.t = <fun>
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.
let make_twist_message v omega=
let open Geometry_msgs in
let mkvector x y z = { vector3_x = x; vector3_y = y; vector3_z = z } in
Twist { twist_linear = mkvector v 0 0 ; twist_angular = mkvector 0 0 omega }
let process_clock_message state =
match state.mode with
| Driving -> { state with outgoing = Some (make_twist_message 10000 0) }
| Turning -> begin
match state.direction with
| None
| Some ( CW ) -> { state with outgoing = Some (make_twist_message 0 10000) }
| Some (CCW ) -> { state with outgoing = Some (make_twist_message 0 (-10000))}
end
val make_twist_message : Z.t -> Z.t -> outgoing_msg = <fun> val process_clock_message : state -> state = <fun>
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:
let get_min_and_direction msg =
let max = msg.Sensor_msgs.laserScan_range_max in
let lst = msg.Sensor_msgs.laserScan_ranges in
let min_range = get_min_range max lst in
let mini = foldi ~base:(max, 0)
(fun i a b -> if a < fst b then (a,i) else b) in
let _ , idx = mini lst in
if idx < List.length lst / 2 then min_range, CW else min_range, CCW
let process_sensor_message state min_range min_direction =
let dirving_state =
{ state with mode = Driving; min_range = None; direction = None } in
let turning_state =
{ state with
mode = Turning
; direction = Some min_direction
; min_range = Some min_range
} in
match state.mode , state.min_range with
| Driving , _ -> if min_range < 20000 then turning_state else dirving_state
| Turning , None -> if min_range > 25000 then dirving_state else turning_state
| Turning , Some old_range ->
if min_range > 25000 then dirving_state
else if min_range > old_range then state else turning_state
val get_min_and_direction : Sensor_msgs.laserScan -> Z.t * direction = <fun> val process_sensor_message : state -> Z.t -> direction -> state = <fun>
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.
let one_step state =
match state.incoming with None -> state | Some in_msg ->
let state = { state with incoming = None; outgoing = None } in
match in_msg with
| Sensor laserScan ->
let min_range, min_direction = get_min_and_direction laserScan in
process_sensor_message state min_range min_direction
| Clock _ -> process_clock_message state
val one_step : state -> state = <fun>
2.3 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:
3. Verifying the ROS node model¶
Formal verification is the process of reasoning mathematically about the correctness of computer programs. We'll use Imandra to formally verify some properties of the ROS node model we've created. First we can use imandra to analyse all possible behaviours of the model, by using Region Decomposition:
Modular_decomp.top "one_step";;
- : Modular_decomp_intf.decomp_ref = <abstr>
No group selected.
- Concrete regions are numbered
- Unnumbered regions are groups whose children share a particular constraint
- Click on a region to view its details
- Double click on a region to zoom in on it
- Shift+double click to zoom out
- Hit escape to reset back to the top
<constraint>
<invariant>
Reg_id | Constraints | Invariant |
---|---|---|
0 | state.incoming = None | state |
1 | Destruct(Some, 0, state.incoming) is Clock state.mode = Driving state.incoming is Some | state.{ incoming = None ; outgoing = Some (Twist Geometry_msgs.{ twist_linear = Geometry_msgs.{ vector3_x = 10000 ; vector3_y = 0 ; vector3_z = 0 } ; twist_angular = Geometry_msgs.{ vector3_x = 0 ; vector3_y = 0 ; vector3_z = 0 } }) ; mode = state.mode ; min_range = state.min_range ; direction = state.direction } |
2 | Destruct(Some, 0, state.incoming) is Clock state.mode = Turning state.direction = None state.incoming is Some | state.{ incoming = None ; outgoing = Some (Twist Geometry_msgs.{ twist_linear = Geometry_msgs.{ vector3_x = 0 ; vector3_y = 0 ; vector3_z = 0 } ; twist_angular = Geometry_msgs.{ vector3_x = 0 ; vector3_y = 0 ; vector3_z = 10000 } }) ; mode = state.mode ; min_range = state.min_range ; direction = state.direction } |
3 | Destruct(Some, 0, state.incoming) is Clock state.mode = Turning Destruct(Some, 0, state.direction) = CW state.direction is Some state.incoming is Some | state.{ incoming = None ; outgoing = Some (Twist Geometry_msgs.{ twist_linear = Geometry_msgs.{ vector3_x = 0 ; vector3_y = 0 ; vector3_z = 0 } ; twist_angular = Geometry_msgs.{ vector3_x = 0 ; vector3_y = 0 ; vector3_z = 10000 } }) ; mode = state.mode ; min_range = state.min_range ; direction = state.direction } |
4 | Destruct(Some, 0, state.incoming) is Clock state.mode = Turning Destruct(Some, 0, state.direction) = CCW state.direction is Some state.incoming is Some | state.{ incoming = None ; outgoing = Some (Twist Geometry_msgs.{ twist_linear = Geometry_msgs.{ vector3_x = 0 ; vector3_y = 0 ; vector3_z = 0 } ; twist_angular = Geometry_msgs.{ vector3_x = 0 ; vector3_y = 0 ; vector3_z = -10000 } }) ; mode = state.mode ; min_range = state.min_range ; direction = state.direction } |
5 | (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] >= ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) <= 25000 Destruct(Some, 0, state.min_range) >= (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning state.min_range is Some state.incoming is Some | state.{ mode = Turning ; min_range = Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) ; direction = Some CCW ; incoming = None ; outgoing = None } |
6 | (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) <= 25000 Destruct(Some, 0, state.min_range) >= (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) > (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] state.min_range is Some state.incoming is Some | state.{ mode = Turning ; min_range = Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) ; direction = Some CW ; incoming = None ; outgoing = None } |
7 | (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] >= ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) <= 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning state.min_range is Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) > Destruct(Some, 0, state.min_range) state.incoming is Some | state.{ incoming = None ; outgoing = None ; mode = state.mode ; min_range = state.min_range ; direction = state.direction } |
8 | (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) <= 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) > (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] state.min_range is Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) > Destruct(Some, 0, state.min_range) state.incoming is Some | state.{ incoming = None ; outgoing = None ; mode = state.mode ; min_range = state.min_range ; direction = state.direction } |
9 | (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] >= ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) > 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning state.min_range is Some state.incoming is Some | state.{ mode = Driving ; min_range = None ; direction = None ; incoming = None ; outgoing = None } |
10 | (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) > 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) > (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] state.min_range is Some state.incoming is Some | state.{ mode = Driving ; min_range = None ; direction = None ; incoming = None ; outgoing = None } |
11 | (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] >= ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) <= 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning state.min_range = None state.incoming is Some | state.{ mode = Turning ; min_range = Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) ; direction = Some CCW ; incoming = None ; outgoing = None } |
12 | (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) <= 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning state.min_range = None ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) > (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] state.incoming is Some | state.{ mode = Turning ; min_range = Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) ; direction = Some CW ; incoming = None ; outgoing = None } |
13 | (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] >= ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) > 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning state.min_range = None state.incoming is Some | state.{ mode = Driving ; min_range = None ; direction = None ; incoming = None ; outgoing = None } |
14 | (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) > 25000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Turning state.min_range = None ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) > (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] state.incoming is Some | state.{ mode = Driving ; min_range = None ; direction = None ; incoming = None ; outgoing = None } |
15 | (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] >= ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) >= 20000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Driving state.incoming is Some | state.{ mode = Driving ; min_range = None ; direction = None ; incoming = None ; outgoing = None } |
16 | (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) >= 20000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Driving ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) > (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] state.incoming is Some | state.{ mode = Driving ; min_range = None ; direction = None ; incoming = None ; outgoing = None } |
17 | (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] >= ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) < 20000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Driving state.incoming is Some | state.{ mode = Turning ; min_range = Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) ; direction = Some CCW ; incoming = None ; outgoing = None } |
18 | (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) < 20000 Destruct(Some, 0, state.incoming) is Sensor state.mode = Driving ((List.length Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) / 2) > (foldi (Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges)[1] state.incoming is Some | state.{ mode = Turning ; min_range = Some (List.fold_right anon_fun.get_min_range.1 Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_range_max Destruct(Sensor, 0, Destruct(Some, 0, state.incoming)) .Sensor_msgs.laserScan_ranges) ; direction = Some CW ; incoming = None ; outgoing = None } |
3.1 Verifying outgoing Twist
message at Clock
ticks¶
Our model is designed in such a way that it updates its state parameters upon LaserScan
messages and sends out Twist
control messages in response to Clock
messages. Let's verify a simple theorem that on every incoming Clock
message, there is an outgoing Twist
message.
We can formally write this statement down as:
$$ \forall s. IsClock(IncomingMessage(s)) \,\Rightarrow\, IsTwist(OutgoingMessage(OneStep(s))) $$eaning that for every state $s$, if the state contains an incoming message and this message is a Clock
message, then the state's outgoing
message is a Twist
after we've called one_step
on it.
We can almost literally encode this formal expression as an Imandra theorem
:
let is_clock msg = match msg with Some ( Clock _ ) -> true | _ -> false ;;
let is_twist msg = match msg with Some ( Twist _ ) -> true | _ -> false ;;
theorem clock_creates_outbound state =
is_clock state.incoming ==> is_twist (one_step state).outgoing
val is_clock : incoming_msg option -> bool = <fun> val is_twist : outgoing_msg option -> bool = <fun> val clock_creates_outbound : state -> bool = <fun>
ground_instances: | 0 | ||||||||||||||||||||||||||||||||
definitions: | 0 | ||||||||||||||||||||||||||||||||
inductions: | 0 | ||||||||||||||||||||||||||||||||
search_time: | 0.009s | ||||||||||||||||||||||||||||||||
details: | Expand
|
start[0.009s] let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : bool) = Is_a(Clock, Option.get _x_0) in (if Is_a(Some, _x_0) then _x_1 else false) ==> Is_a(Some, (if Is_a(None, _x_0) then ( :var_0: ) else if _x_1 then … else …).outgoing)
simplify
into: let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : bool) = Is_a(Clock, Option.get _x_0) in not (Is_a(Some, _x_0) && _x_1) || Is_a(Some, (if Is_a(None, _x_0) then ( :var_0: ) else if _x_1 then … else …).outgoing)
expansions: []
rewrite_steps: forward_chaining: - Unsat
One can see that Imandra says that it "Proved" the theorem, meaning that Imandra has formally checked that this property holds for all possible input states.
3.2 Verifying that we never drive backwards¶
As another example, let us check that, no matter what, the node will never send out a message with negative linear velocity.
let no_moving_back msg =
let open Geometry_msgs in
match msg with None -> true
| Some (Twist data) -> data.twist_linear.vector3_x >= 0
verify ( fun state -> no_moving_back (one_step state).outgoing )
val no_moving_back : outgoing_msg option -> bool = <fun> - : state -> bool = <fun> module CX : sig val state : state end
Counterexample (after 0 steps, 0.009s): let state : state = {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = (-7720); Geometry_msgs.vector3_y = 8; Geometry_msgs.vector3_z = 9}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 10; Geometry_msgs.vector3_y = 11; Geometry_msgs.vector3_z = 12}})}
ground_instances: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||
definitions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||
inductions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||
search_time: | 0.009s | ||||||||||||||||||||||||||||||||||||||||||||||||||
details: | Expand
|
start[0.009s] if Is_a(None, (if Is_a(None, ( :var_0: ).incoming) then ( :var_0: ) else if Is_a(Clock, …) then … else …).outgoing) then true else (Destruct(Twist, 0, …)).Geometry_msgs.twist_linear.Geometry_msgs.vector3_x >= 0
simplify
into: let (_x_0 : outgoing_msg option) = (if Is_a(None, ( :var_0: ).incoming) then ( :var_0: ) else if Is_a(Clock, …) then … else …).outgoing in Is_a(None, _x_0) || ((Destruct(Twist, 0, Option.get _x_0)).Geometry_msgs.twist_linear.Geometry_msgs.vector3_x >= 0)
expansions: []
rewrite_steps: forward_chaining: - Sat (Some let state : state = {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = (Z.of_nativeint (-7720n)); Geometry_msgs.vector3_y = (Z.of_nativeint (8n)); Geometry_msgs.vector3_z = (Z.of_nativeint (9n))}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = (Z.of_nativeint (10n)); Geometry_msgs.vector3_y = (Z.of_nativeint (11n)); Geometry_msgs.vector3_z = (Z.of_nativeint (12n))}})} )
We have failed to prove the statement and Imandra have created a counterexample CX
module for us. This module contains concrete values for the parameters of the verified statement, that violate the statement's condition. Let's examine the value of CX.state
:
CX.state
- : state = {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = -7720; vector3_y = 8; vector3_z = 9}; twist_angular = {Geometry_msgs.vector3_x = 10; vector3_y = 11; vector3_z = 12}})}
The counterexample state
produced by imandra has the incoming
message set to None
and the outgoing
message set to a Twist
message with negative linear.x
. Our one_step
function keeps the state unchanged if the incoming message is empty.
We can either consider this behavior as a bug and change our one_step
implementation; or we can consider this a normal behavior and amend our theorem, adding the non-empty incoming message as an extra premise of the theorem:
theorem never_goes_back state =
state.incoming <> None
==>
no_moving_back (one_step state).outgoing
val never_goes_back : state -> bool = <fun>
ground_instances: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||
definitions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||
inductions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||
search_time: | 0.009s | ||||||||||||||||||||||||||||||||||||||||||||||||
details: | Expand
|
start[0.009s] let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in not (_x_0 = None) ==> (if Is_a(None, (if Is_a(None, _x_0) then ( :var_0: ) else if Is_a(Clock, …) then … else …).outgoing) then true else (Destruct(Twist, 0, …)).Geometry_msgs.twist_linear.Geometry_msgs.vector3_x >= 0)
simplify
into: let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : outgoing_msg option) = (if Is_a(None, _x_0) then ( :var_0: ) else if Is_a(Clock, …) then … else …).outgoing in (_x_0 = None) || Is_a(None, _x_1) || ((Destruct(Twist, 0, Option.get _x_1)).Geometry_msgs.twist_linear.Geometry_msgs.vector3_x >= 0)
expansions: []
rewrite_steps: forward_chaining: - Unsat
We've proven that the model never creates negative linear speed in response to any incoming message - alternatively we can set state.outgoing = None
as a premise, proving that an empty outgoing
message is never filled with a Twist
with negative velocity:
theorem never_goes_back_alt state =
state.outgoing = None
==>
no_moving_back (one_step state).outgoing
val never_goes_back_alt : state -> bool = <fun>
ground_instances: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||
definitions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||
inductions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||
search_time: | 0.008s | ||||||||||||||||||||||||||||||||||||||||||||||
details: | Expand
|
start[0.008s] ( :var_0: ).outgoing = None ==> (if Is_a(None, (if Is_a(None, ( :var_0: ).incoming) then ( :var_0: ) else if Is_a(Clock, …) then … else …).outgoing) then true else (Destruct(Twist, 0, …)).Geometry_msgs.twist_linear.Geometry_msgs.vector3_x >= 0)
simplify
into: let (_x_0 : outgoing_msg option) = (if Is_a(None, ( :var_0: ).incoming) then ( :var_0: ) else if Is_a(Clock, …) then … else …).outgoing in Is_a(None, _x_0) || ((Destruct(Twist, 0, Option.get _x_0)).Geometry_msgs.twist_linear.Geometry_msgs.vector3_x >= 0) || not (( :var_0: ).outgoing = None)
expansions: []
rewrite_steps: forward_chaining: - Unsat
4. Inductive proofs. Stopping near objects.¶
As a final formal verification goal, we want to be able to prove that the robot stops and starts turning if one of the values in the scanner ranges
is lower than 0.2 meters. In general, reasoning about variable-sized lists requires inductive proofs - and these might require proving some lemmas to guide Imandra to the proof. So, we will first try to prove a simpler version of the theorem - if all the ranges in the incoming laser scan message are less than 0.2 meters, then we definitely transition to the Turning
state. We'll try to encode our theorem using List.for_all
standard function:
verify ( fun state ->
let open Sensor_msgs in
match state.incoming with None | Some (Clock _ ) -> true
| Some ( Sensor data ) ->
( List.for_all (fun x -> x < 20000) data.laserScan_ranges
) ==> (one_step state).mode = Turning
)
- : state -> bool = <fun> module CX : sig val state : state end
Counterexample (after 4 steps, 0.012s): let state : state = {mode = Driving; min_range = None; direction = None; incoming = Some (Sensor {Sensor_msgs.laserScan_range_min = 3; Sensor_msgs.laserScan_range_max = 20000; Sensor_msgs.laserScan_ranges = []}); outgoing = None}
ground_instances: | 4 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
definitions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
inductions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
search_time: | 0.012s | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
details: | Expand
|
start[0.012s] let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : bool) = Is_a(None, _x_0) in if _x_1 then true else if Is_a(Clock, Option.get _x_0) then true else List.for_all anon_fun._verify_target.1 ….Sensor_msgs.laserScan_ranges ==> (if _x_1 then ( :var_0: ) else …).mode = Turning
simplify
into: let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : bool) = Is_a(Clock, Option.get _x_0) in let (_x_2 : bool) = Is_a(None, _x_0) in let (_x_3 : mode) = ( :var_0: ).mode in _x_1 || _x_2 || ((if _x_2 then ( :var_0: ) else if _x_1 then if _x_3 = Driving then … else … else if Is_a(Driving, _x_3) then … else …).mode = Turning) || not (List.for_all anon_fun._verify_target.1 ….Sensor_msgs.laserScan_ranges)
expansions: []
rewrite_steps: forward_chaining: - unroll
expr: (let ((a!1 (|Sensor_msgs.laserScan_range_max_1271/client| (|get.Sensor.0_1031/server| …
expansions: - unroll
expr: (let ((a!1 (|Sensor_msgs.laserScan_ranges_1272/client| (|get.Sensor.0_1031/server| …
expansions: - unroll
expr: (let ((a!1 (|Sensor_msgs.laserScan_range_max_1271/client| (|get.Sensor.0_1031/server| …
expansions: - unroll
expr: (let ((a!1 (|Sensor_msgs.laserScan_ranges_1272/client| (|get.Sensor.0_1031/server| …
expansions: - Sat (Some let state : state = {mode = Driving; min_range = None; direction = None; incoming = Some (Sensor {Sensor_msgs.laserScan_range_min = (Z.of_nativeint (3n)); Sensor_msgs.laserScan_range_max = (Z.of_nativeint (20000n)); Sensor_msgs.laserScan_ranges = []}); outgoing = None} )
We have failed to prove the statement and Imandra has created a counterexample CX
module for us. Examining the counterexample state we notice that the incoming laserScan_ranges list
is empty.
CX.state
- : state = {mode = Driving; min_range = None; direction = None; incoming = Some (Sensor {Sensor_msgs.laserScan_range_min = 3; laserScan_range_max = 20000; laserScan_ranges = []}); outgoing = None}
Adding the extra requirement that the list is not []
, we successfully verify the statement:
theorem stopping_if_for_all state =
let open Sensor_msgs in
match state.incoming with None | Some (Clock _ ) -> true
| Some ( Sensor data ) ->
( data.laserScan_ranges <> []
&& List.for_all (fun x -> x < 20000) data.laserScan_ranges
) ==> (one_step state).mode = Turning
val stopping_if_for_all : state -> bool = <fun>
ground_instances: | 2 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
definitions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
inductions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
search_time: | 0.009s | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
details: | Expand
|
start[0.009s] let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : incoming_msg) = Option.get _x_0 in let (_x_2 : int list) = (Destruct(Sensor, 0, _x_1)).Sensor_msgs.laserScan_ranges in let (_x_3 : bool) = Is_a(None, _x_0) in if _x_3 then true else if Is_a(Clock, _x_1) then true else not (_x_2 = []) && List.for_all anon_fun._verify_target.1 _x_2 ==> (if _x_3 then ( :var_0: ) else …).mode = Turning
simplify
into: let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : incoming_msg) = Option.get _x_0 in let (_x_2 : bool) = Is_a(Clock, _x_1) in let (_x_3 : bool) = Is_a(None, _x_0) in let (_x_4 : mode) = ( :var_0: ).mode in let (_x_5 : int list) = (Destruct(Sensor, 0, _x_1)).Sensor_msgs.laserScan_ranges in _x_2 || _x_3 || ((if _x_3 then ( :var_0: ) else if _x_2 then if _x_4 = Driving then … else … else if Is_a(Driving, _x_4) then … else …).mode = Turning) || not (not (_x_5 = []) && List.for_all anon_fun._verify_target.1 _x_5)
expansions: []
rewrite_steps: forward_chaining: - unroll
expr: (let ((a!1 (|Sensor_msgs.laserScan_ranges_1272/client| (|get.Sensor.0_1172/server| …
expansions: - unroll
expr: (let ((a!1 (|Sensor_msgs.laserScan_range_max_1271/client| (|get.Sensor.0_1172/server| …
expansions: - Unsat
Imandra successfully proves the stopping_if_for_all
theorem, but our ultimate goal is to prove the theorem when some of the values in laserScan_ranges
are less than the cutoff. If we simply try to replace the List.for_all
with List.exists
, Imandra will fail to either prove or disprove the theorem. The inductive structure of this proof is too complex for Imandra to figure out automatically without any hints from the user. We need to help it with the overall logic of the proof. To do that we will break this final theorem into several smaller steps, making a rough "sketch" of the inductive proof we want and and ask Imandra to fill in the gaps.
As a first step, we extract the anonymous threshold function and prove a lemma that if the get_min_range
function returns a value satisfying the threshold, then the conclusion about the one_step
function holds:
let under_threshold x = x < 20000
lemma lemma1 state =
let open Sensor_msgs in
match state.incoming with None | Some (Clock _ ) -> true
| Some ( Sensor data ) ->
( data.laserScan_ranges <> []
&& under_threshold (get_min_range data.laserScan_range_max data.laserScan_ranges)
) ==> (one_step state).mode = Turning
val under_threshold : Z.t -> bool = <fun> val lemma1 : state -> bool = <fun>
ground_instances: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||
definitions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||
inductions: | 0 | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||
search_time: | 0.008s | ||||||||||||||||||||||||||||||||||||||||||||||||||||||||
details: | Expand
|
start[0.008s] let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : incoming_msg) = Option.get _x_0 in let (_x_2 : Sensor_msgs.laserScan) = Destruct(Sensor, 0, _x_1) in let (_x_3 : int list) = _x_2.Sensor_msgs.laserScan_ranges in let (_x_4 : bool) = Is_a(None, _x_0) in if _x_4 then true else if Is_a(Clock, _x_1) then true else not (_x_3 = []) && (List.fold_right anon_fun.get_min_range.1 _x_2.Sensor_msgs.laserScan_range_max _x_3 < 20000) ==> (if _x_4 then ( :var_0: ) else …).mode = Turning
simplify
into: let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : incoming_msg) = Option.get _x_0 in let (_x_2 : bool) = Is_a(Clock, _x_1) in let (_x_3 : bool) = Is_a(None, _x_0) in let (_x_4 : mode) = ( :var_0: ).mode in let (_x_5 : Sensor_msgs.laserScan) = Destruct(Sensor, 0, _x_1) in let (_x_6 : int list) = _x_5.Sensor_msgs.laserScan_ranges in _x_2 || _x_3 || ((if _x_3 then ( :var_0: ) else if _x_2 then if _x_4 = Driving then … else … else if Is_a(Driving, _x_4) then … else …).mode = Turning) || not (not (_x_6 = []) && not (20000 <= List.fold_right anon_fun.get_min_range.1 _x_5.Sensor_msgs.laserScan_range_max _x_6))
expansions: []
rewrite_steps: forward_chaining: - Unsat
Next, we prove a "bridge" lemma that translates between the get_min_range
concept and the List.exists
concept for the under_threshold
function.
lemma bridge max lst =
List.exists under_threshold lst ==> under_threshold (get_min_range max lst)
[@@induct]
val bridge : Z.t -> Z.t list -> bool = <fun> Goal: List.exists under_threshold lst ==> under_threshold (get_min_range max lst). 1 nontautological subgoal. Subgoal 1: H0. List.exists under_threshold lst H1. 20000 <= List.fold_right anon_fun.get_min_range.1 max lst |--------------------------------------------------------------------------- false Must try induction. The recursive terms in the conjecture suggest 2 inductions. Subsumption and merging reduces this to 1. We shall induct according to a scheme derived from List.fold_right. Induction scheme: (not (not (List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && lst <> []) && not ((List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && lst <> []) ==> φ lst max) && ((lst <> [] && ((List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && φ (List.tl lst) max) ==> φ lst max) && (lst <> [] && (not (List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && (φ (List.tl lst) max && φ (List.tl lst) max)) ==> φ lst max)). 3 nontautological subgoals. Subgoal 1.3: H0. 20000 <= List.fold_right anon_fun.get_min_range.1 max lst H1. List.exists under_threshold lst H2. not ((List.fold_right anon_fun.get_min_range.1 max (List.tl lst) <= List.hd lst) && lst <> []) H3. not (not (List.fold_right anon_fun.get_min_range.1 max (List.tl lst) <= List.hd lst) && lst <> []) |--------------------------------------------------------------------------- false But simplification reduces this to true, using the definition of List.exists. Subgoal 1.2: H0. 20000 <= List.fold_right anon_fun.get_min_range.1 max lst H1. List.exists under_threshold lst H2. lst <> [] H3. not (List.fold_right anon_fun.get_min_range.1 max (List.tl lst) <= List.hd lst) H4. not (List.exists under_threshold (List.tl lst)) || not (20000 <= List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) |--------------------------------------------------------------------------- false But simplification reduces this to true, using the definitions of List.exists and List.fold_right. Subgoal 1.1: H0. 20000 <= List.fold_right anon_fun.get_min_range.1 max lst H1. List.exists under_threshold lst H2. lst <> [] H3. List.fold_right anon_fun.get_min_range.1 max (List.tl lst) <= List.hd lst H4. not (List.exists under_threshold (List.tl lst)) || not (20000 <= List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) |--------------------------------------------------------------------------- false But simplification reduces this to true, using the definitions of List.exists and List.fold_right. ⓘ Rules: (:def List.exists) (:def List.fold_right) (:induct List.fold_right)
ground_instances: | 0 |
definitions: | 10 |
inductions: | 1 |
search_time: | 0.223s |
start[0.223s, "Goal"] List.exists under_threshold ( :var_0: ) ==> List.fold_right anon_fun.get_min_range.1 ( :var_1: ) ( :var_0: ) < 20000
subproof
not (List.exists under_threshold lst) || not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst)start[0.223s, "1"] not (List.exists under_threshold lst) || not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst)
induction on (functional ?) :scheme (not (not (List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && lst <> []) && not ((List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && lst <> []) ==> φ lst max) && ((lst <> [] && ((List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && φ (List.tl lst) max) ==> φ lst max) && (lst <> [] && (not (List.hd lst < List.fold_right anon_fun.get_min_range.1 max (List.tl lst)) && (φ (List.tl lst) max && φ (List.tl lst) max)) ==> φ lst max))
Split (let (_x_0 : bool) = not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) in let (_x_1 : int list) = List.tl lst in let (_x_2 : int) = List.fold_right anon_fun.get_min_range.1 max _x_1 in let (_x_3 : bool) = _x_2 <= List.hd lst in let (_x_4 : bool) = lst <> [] in let (_x_5 : bool) = not _x_3 in let (_x_6 : bool) = not (List.exists under_threshold _x_1) || not (20000 <= _x_2) in (_x_0 || not (not (_x_3 && _x_4) && not (_x_5 && _x_4))) && (_x_0 || not (_x_4 && _x_5 && _x_6)) && (_x_0 || not (_x_4 && _x_3 && _x_6)) :cases [let (_x_0 : bool) = List.fold_right anon_fun.get_min_range.1 max (List.tl lst) <= List.hd lst in let (_x_1 : bool) = lst <> [] in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || (_x_0 && _x_1) || (not _x_0 && _x_1); let (_x_0 : int list) = List.tl lst in let (_x_1 : int) = List.fold_right anon_fun.get_min_range.1 max _x_0 in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || not (lst <> []) || (_x_1 <= List.hd lst) || not (not (List.exists under_threshold _x_0) || not (20000 <= _x_1)); let (_x_0 : int list) = List.tl lst in let (_x_1 : int) = List.fold_right anon_fun.get_min_range.1 max _x_0 in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || not (lst <> []) || not (_x_1 <= List.hd lst) || not (not (List.exists under_threshold _x_0) || not (20000 <= _x_1))])
subproof
let (_x_0 : int list) = List.tl lst in let (_x_1 : int) = List.fold_right anon_fun.get_min_range.1 max _x_0 in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || not (lst <> []) || not (_x_1 <= List.hd lst) || not (not (List.exists under_threshold _x_0) || not (20000 <= _x_1))start[0.180s, "1.1"] let (_x_0 : int list) = List.tl lst in let (_x_1 : int) = List.fold_right anon_fun.get_min_range.1 max _x_0 in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || not (lst <> []) || not (_x_1 <= List.hd lst) || not (not (List.exists under_threshold _x_0) || not (20000 <= _x_1))
simplify
into: true
expansions: [List.fold_right, List.exists, List.fold_right, List.exists]
rewrite_steps: forward_chaining:
subproof
let (_x_0 : int list) = List.tl lst in let (_x_1 : int) = List.fold_right anon_fun.get_min_range.1 max _x_0 in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || not (lst <> []) || (_x_1 <= List.hd lst) || not (not (List.exists under_threshold _x_0) || not (20000 <= _x_1))start[0.180s, "1.2"] let (_x_0 : int list) = List.tl lst in let (_x_1 : int) = List.fold_right anon_fun.get_min_range.1 max _x_0 in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || not (lst <> []) || (_x_1 <= List.hd lst) || not (not (List.exists under_threshold _x_0) || not (20000 <= _x_1))
simplify
into: true
expansions: [List.fold_right, List.exists, List.fold_right, List.exists]
rewrite_steps: forward_chaining:
subproof
let (_x_0 : bool) = List.fold_right anon_fun.get_min_range.1 max (List.tl lst) <= List.hd lst in let (_x_1 : bool) = lst <> [] in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || (_x_0 && _x_1) || (not _x_0 && _x_1)start[0.180s, "1.3"] let (_x_0 : bool) = List.fold_right anon_fun.get_min_range.1 max (List.tl lst) <= List.hd lst in let (_x_1 : bool) = lst <> [] in not (20000 <= List.fold_right anon_fun.get_min_range.1 max lst) || not (List.exists under_threshold lst) || (_x_0 && _x_1) || (not _x_0 && _x_1)
simplify
into: true
expansions: [List.exists, List.exists]
rewrite_steps: forward_chaining:
Then, we [@@apply]
the two lemmas above to prove our final theorem with the List.exists
condition: these proofs require induction - to tell Imandra to use induction one should add the [@@induct]
attribute to the theorem declaration:
theorem stopping_if_exists state =
let open Sensor_msgs in
match state.incoming with None | Some (Clock _ ) -> true
| Some ( Sensor data ) ->
( data.laserScan_ranges <> []
&& List.exists under_threshold data.laserScan_ranges
) ==> (one_step state).mode = Turning
[@@apply lemma1 state]
[@@apply bridge
(match state.incoming with Some (Sensor data) -> data.laserScan_range_max | _ -> 0)
(match state.incoming with Some (Sensor data) -> data.laserScan_ranges | _ -> []) ]
[@@induct]
val stopping_if_exists : state -> bool = <fun> Goal: begin match (state.incoming : incoming_msg option) with | None -> true | Some (_pat_116 : incoming_msg) -> begin match (_pat_116 : incoming_msg) with | Clock (_pat_117 : Rosgraph_msgs.clock) -> true | Sensor (data : Sensor_msgs.laserScan) -> (data.Sensor_msgs.laserScan_ranges <> []) && List.exists under_threshold data.Sensor_msgs.laserScan_ranges ==> (one_step state).mode = Turning end end. A hint has been given, resulting in the following augmented conjecture: Goal': (if Is_a(None, state.incoming) then true else if Is_a(Clock, Option.get state.incoming) then true else not ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges = []) && (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges < 20000) ==> (if Is_a(None, state.incoming) then state else if Is_a(Clock, Option.get state.incoming) then if state.mode = Driving then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 10000; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}})} else if Is_a(None, state.direction) then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else if Option.get state.direction = CW then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = (-10000)}})} else if Is_a(Driving, state.mode) then if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 < 20000 then {mode = Turning; min_range = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0; direction = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).1; incoming = None; outgoing = None} else {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else if Is_a(None, state.min_range) then if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 > 25000 then {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else {mode = Turning; min_range = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0; direction = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).1; incoming = None; outgoing = None} else if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 > 25000 then {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 > Option.get state.min_range then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = None} else {mode = Turning; min_range = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0; direction = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).1; incoming = None; outgoing = None}).mode = Turning) && (List.exists under_threshold (if Is_a(Some, state.incoming) then if Is_a(Sensor, Option.get state.incoming) then (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges else [] else []) ==> List.fold_right anon_fun.get_min_range.1 (if Is_a(Some, state.incoming) then if Is_a(Sensor, Option.get state.incoming) then (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max else 0 else 0) (if Is_a(Some, state.incoming) then if Is_a(Sensor, Option.get state.incoming) then (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges else [] else []) < 20000) ==> (if Is_a(None, state.incoming) then true else if Is_a(Clock, Option.get state.incoming) then true else not ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges = []) && List.exists under_threshold (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges ==> (if Is_a(None, state.incoming) then state else if Is_a(Clock, Option.get state.incoming) then if state.mode = Driving then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 10000; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}})} else if Is_a(None, state.direction) then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else if Option.get state.direction = CW then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = (-10000)}})} else if Is_a(Driving, state.mode) then if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 < 20000 then {mode = Turning; min_range = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0; direction = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).1; incoming = None; outgoing = None} else {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else if Is_a(None, state.min_range) then if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 > 25000 then {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else {mode = Turning; min_range = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0; direction = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).1; incoming = None; outgoing = None} else if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 > 25000 then {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else if (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0 > Option.get state.min_range then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = None} else {mode = Turning; min_range = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).0; direction = Some (if (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 < List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW)).1; incoming = None; outgoing = None}).mode = Turning). 1 nontautological subgoal. Subgoal 1: H0. Is_a(None, state.incoming) || Is_a(Clock, Option.get state.incoming) || not (not ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges = []) && not (20000 <= List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges)) || ((if Is_a(None, state.incoming) then state else if Is_a(Clock, Option.get state.incoming) then if state.mode = Driving then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 10000; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}})} else if Is_a(None, state.direction) || (Option.get state.direction = CW) then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = (-10000)}})} else if Is_a(Driving, state.mode) then if 20000 <= (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 then {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else {mode = Turning; min_range = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0; direction = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).1; incoming = None; outgoing = None} else if Is_a(None, state.min_range) then if (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 <= 25000 then {mode = Turning; min_range = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0; direction = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).1; incoming = None; outgoing = None} else {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else if (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 <= 25000 then if (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 <= Option.get state.min_range then {mode = Turning; min_range = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0; direction = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).1; incoming = None; outgoing = None} else {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = None} else {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None}).mode = Turning) H1. not (List.exists under_threshold (if Is_a(Some, state.incoming) then if Is_a(Sensor, Option.get state.incoming) then (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges else [] else [])) || not (20000 <= List.fold_right anon_fun.get_min_range.1 (if Is_a(Some, state.incoming) then if Is_a(Sensor, Option.get state.incoming) then (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max else 0 else 0) (if Is_a(Some, state.incoming) then if Is_a(Sensor, Option.get state.incoming) then (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges else [] else [])) H2. not ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges = []) H3. List.exists under_threshold (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges |--------------------------------------------------------------------------- C0. Is_a(None, state.incoming) C1. Is_a(Clock, Option.get state.incoming) C2. (if Is_a(None, state.incoming) then state else if Is_a(Clock, Option.get state.incoming) then if state.mode = Driving then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 10000; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}})} else if Is_a(None, state.direction) || (Option.get state.direction = CW) then {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = (-10000)}})} else if Is_a(Driving, state.mode) then if 20000 <= (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 then {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else {mode = Turning; min_range = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0; direction = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).1; incoming = None; outgoing = None} else if Is_a(None, state.min_range) then if (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 <= 25000 then {mode = Turning; min_range = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0; direction = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).1; incoming = None; outgoing = None} else {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} else if (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 <= 25000 then if (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0 <= Option.get state.min_range then {mode = Turning; min_range = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).0; direction = Some (if List.length (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges / 2 <= (foldi ((Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max, 0) None anon_fun.get_min_and_direction.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges).1 then (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CCW) else (List.fold_right anon_fun.get_min_range.1 (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_range_max (Destruct(Sensor, 0, Option.get state.incoming)).Sensor_msgs.laserScan_ranges, CW)).1; incoming = None; outgoing = None} else {mode = state.mode; min_range = state.min_range; direction = state.direction; incoming = None; outgoing = None} else {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None}).mode = Turning But simplification reduces this to true, using the forward-chaining rule List.len_nonnegative. ⓘ Rules: (:fc List.len_nonnegative) (:app apply_hint.stopping_if_exists.0) (:app apply_hint.stopping_if_exists.1)
ground_instances: | 0 |
definitions: | 0 |
inductions: | 0 |
search_time: | 0.065s |
start[0.065s, "Goal"] let (_x_0 : incoming_msg option) = ( :var_0: ).incoming in let (_x_1 : incoming_msg) = Option.get _x_0 in let (_x_2 : Sensor_msgs.laserScan) = Destruct(Sensor, 0, _x_1) in let (_x_3 : int list) = _x_2.Sensor_msgs.laserScan_ranges in let (_x_4 : bool) = not (_x_3 = []) in let (_x_5 : int) = _x_2.Sensor_msgs.laserScan_range_max in let (_x_6 : bool) = Is_a(None, _x_0) in let (_x_7 : bool) = (if _x_6 then ( :var_0: ) else …).mode = Turning in let (_x_8 : bool) = Is_a(Clock, _x_1) in let (_x_9 : bool) = Is_a(Sensor, _x_1) in let (_x_10 : bool) = Is_a(Some, _x_0) in let (_x_11 : int list) = if _x_10 then if _x_9 then _x_3 else [] else [] in (if _x_6 then true else if _x_8 then true else _x_4 && (List.fold_right anon_fun.get_min_range.1 _x_5 _x_3 < 20000) ==> _x_7) && (List.exists under_threshold _x_11 ==> List.fold_right anon_fun.get_min_range.1 (if _x_10 then if _x_9 then _x_5 else 0 else 0) _x_11 < 20000) ==> (if _x_6 then true else if _x_8 then true else _x_4 && List.exists under_threshold _x_3 ==> _x_7)
subproof
let (_x_0 : incoming_msg option) = state.incoming in let (_x_1 : bool) = Is_a(None, _x_0) in let (_x_2 : incoming_msg) = Option.get _x_0 in let (_x_3 : bool) = Is_a(Clock, _x_2) in let (_x_4 : Sensor_msgs.laserScan) = Destruct(Sensor, 0, _x_2) in let (_x_5 : int list) = _x_4.Sensor_msgs.laserScan_ranges in let (_x_6 : int) = _x_4.Sensor_msgs.laserScan_range_max in let (_x_7 : int) = List.fold_right anon_fun.get_min_range.1 _x_6 _x_5 in let (_x_8 : state) = {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} in let (_x_9 : mode) = state.mode in let (_x_10 : int option) = state.min_range in let (_x_11 : direction option) = state.direction in let (_x_12 : (int * direction)) = if List.length _x_5 / 2 <= (foldi (_x_6, 0) None anon_fun.get_min_and_direction.1 _x_5).1 then (_x_7, CCW) else (_x_7, CW) in let (_x_13 : int) = _x_12.0 in let (_x_14 : state) = {mode = Turning; min_range = Some _x_13; direction = Some _x_12.1; incoming = None; outgoing = None} in let (_x_15 : bool) = _x_13 <= 25000 in let (_x_16 : Geometry_msgs.vector3) = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0} in let (_x_17 : bool) = (if _x_1 then state else if _x_3 then if _x_9 = Driving then {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 10000; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = _x_16})} else if Is_a(None, _x_11) || (Option.get _x_11 = CW) then {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = _x_16; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = _x_16; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = (-10000)}})} else if Is_a(Driving, _x_9) then if 20000 <= _x_13 then _x_8 else _x_14 else if Is_a(None, _x_10) then if _x_15 then _x_14 else _x_8 else if _x_15 then if _x_13 <= Option.get _x_10 then _x_14 else {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = None} else _x_8).mode = Turning in let (_x_18 : bool) = Is_a(Sensor, _x_2) in let (_x_19 : bool) = Is_a(Some, _x_0) in let (_x_20 : int list) = if _x_19 then if _x_18 then _x_5 else [] else [] in not (_x_1 || _x_3 || not (not (_x_5 = []) && not (20000 <= _x_7)) || _x_17) || not (not (List.exists under_threshold _x_20) || not (20000 <= List.fold_right anon_fun.get_min_range.1 (if _x_19 then if _x_18 then _x_6 else 0 else 0) _x_20)) || ((Destruct(Sensor, 0, Option.get ….incoming)).Sensor_msgs.laserScan_ranges = []) || not (List.exists under_threshold _x_5) || _x_1 || _x_3 || _x_17start[0.064s, "1"] let (_x_0 : incoming_msg option) = state.incoming in let (_x_1 : bool) = Is_a(None, _x_0) in let (_x_2 : incoming_msg) = Option.get _x_0 in let (_x_3 : bool) = Is_a(Clock, _x_2) in let (_x_4 : Sensor_msgs.laserScan) = Destruct(Sensor, 0, _x_2) in let (_x_5 : int list) = _x_4.Sensor_msgs.laserScan_ranges in let (_x_6 : int) = _x_4.Sensor_msgs.laserScan_range_max in let (_x_7 : int) = List.fold_right anon_fun.get_min_range.1 _x_6 _x_5 in let (_x_8 : state) = {mode = Driving; min_range = None; direction = None; incoming = None; outgoing = None} in let (_x_9 : mode) = state.mode in let (_x_10 : int option) = state.min_range in let (_x_11 : direction option) = state.direction in let (_x_12 : (int * direction)) = if List.length _x_5 / 2 <= (foldi (_x_6, 0) None anon_fun.get_min_and_direction.1 _x_5).1 then (_x_7, CCW) else (_x_7, CW) in let (_x_13 : int) = _x_12.0 in let (_x_14 : state) = {mode = Turning; min_range = Some _x_13; direction = Some _x_12.1; incoming = None; outgoing = None} in let (_x_15 : bool) = _x_13 <= 25000 in let (_x_16 : Geometry_msgs.vector3) = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0} in let (_x_17 : bool) = (if _x_1 then state else if _x_3 then if _x_9 = Driving then {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = {Geometry_msgs.vector3_x = 10000; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 0}; Geometry_msgs.twist_angular = _x_16})} else if Is_a(None, _x_11) || (Option.get _x_11 = CW) then {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = _x_16; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = 10000}})} else {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = Some (Twist {Geometry_msgs.twist_linear = _x_16; Geometry_msgs.twist_angular = {Geometry_msgs.vector3_x = 0; Geometry_msgs.vector3_y = 0; Geometry_msgs.vector3_z = (-10000)}})} else if Is_a(Driving, _x_9) then if 20000 <= _x_13 then _x_8 else _x_14 else if Is_a(None, _x_10) then if _x_15 then _x_14 else _x_8 else if _x_15 then if _x_13 <= Option.get _x_10 then _x_14 else {mode = _x_9; min_range = _x_10; direction = _x_11; incoming = None; outgoing = None} else _x_8).mode = Turning in let (_x_18 : bool) = Is_a(Sensor, _x_2) in let (_x_19 : bool) = Is_a(Some, _x_0) in let (_x_20 : int list) = if _x_19 then if _x_18 then _x_5 else [] else [] in not (_x_1 || _x_3 || not (not (_x_5 = []) && not (20000 <= _x_7)) || _x_17) || not (not (List.exists under_threshold _x_20) || not (20000 <= List.fold_right anon_fun.get_min_range.1 (if _x_19 then if _x_18 then _x_6 else 0 else 0) _x_20)) || ((Destruct(Sensor, 0, Option.get ….incoming)).Sensor_msgs.laserScan_ranges = []) || not (List.exists under_threshold _x_5) || _x_1 || _x_3 || _x_17
simplify
into: true
expansions: []
rewrite_steps: forward_chaining: List.len_nonnegative