process_sensor_message.ml

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 driving_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 driving_state
  | Turning , None -> if min_range > 25000 then driving_state else turning_state
  | Turning , Some old_range -> 
    if min_range > 25000 then driving_state
    else if min_range > old_range then state else turning_state