KKostya

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

KKostya

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)

KKostya

lemma bridge max lst =
  List.exists under_threshold lst ==> under_threshold (get_min_range max lst)
  [@@induct]

KKostya

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

KKostya

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 

KKostya

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 =

KKostya

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

KKostya

let get_min_range max lst =
  List.fold_right ~base:max
    (fun x a -> if x < a then x else a) lst

KKostya

let rec foldi ~base ?(i=0) f l =
  match l with
  | [] -> base
  | x :: tail -> f i x ( foldi f ~base ~i:(i+1) tail )

KKostya

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