1. bergsoe
  2. papl

Commits

bergsoe  committed 0d70b02

RRT planners with arbitrary target callbacks.

  • Participants
  • Parent commits 5a1c12d
  • Branches master

Comments (0)

Files changed (2)

File src/PaplRRT.ml

View file
  • Ignore whitespace
 module Tree = PaplTree
 module TP = PaplTreePlanner
 
+type callback_t = unit -> unit
+
 type 'a status_t =
     Advanced of bool * 'a
   | Blocked
 
-module type SETUP = sig
+module type BASE = sig
   type q_t
   type value_t
 
   val get_q : value_t -> q_t
 end
 
-module MakeTPSetup (S : SETUP) = struct
+module type SETUP = sig
+  include BASE
+  type target_t
+
+  val create_callback : target_t -> Index.t -> callback_t
+end
+
+module MakeTPSetup (S : BASE) = struct
   module SI = S.Index
 
   type t = {
     buf : SI.t;
-    callback : t -> unit;
+    callback : callback_t;
   }
 
   type q_t = S.q_t
 
   let get_q = S.get_q
 
-  let create callback = {
-    buf = SI.create ();
-    callback = callback;
-  }
-
-  let add_root s q = SI.add_root s.buf (S.make_value q)
-
   let add_qs s parent qs =
     List.fold_left
       (fun parent q -> SI.add s.buf parent (S.make_value q))
                   Advanced (reached, node)
 
   let expand s =
-    s.callback s;
+    s.callback ();
     match BatEnum.get S.sampler with
         None -> PaplPlanner.fail "RRT extension sampler is empty"
       | Some None -> None
       | Advanced (true, node) -> Some node
       | _ -> None
 
+  let add_root buf q = SI.add_root buf (S.make_value q)
+
+  let create buf callback = {
+    buf = buf;
+    callback = callback;
+  }
+
+  let create_callback build =
+    let buf = SI.create () in
+    let callback = build buf in 
+      create buf callback
+
   let create_point q =
-    let s = create (fun _ -> ()) in
-    let _ = add_root s q in
-      s
+    let buf = SI.create () in
+    let _ = add_root buf q in
+    let callback () = () in
+      create buf callback
 
   let create_region sampler =
-    let add s q = let _ = add_root s q in () in
-    let callback s =
+    let buf = SI.create () in
+    let add q = let _ = add_root buf q in () in
+    let callback () =
       BatOption.may
-        (List.iter (fun q -> add s q))
+        (List.iter (fun q -> add q))
         (BatEnum.get sampler)
-    in {
-        buf = SI.create ();
-        callback = callback;
-      }
+    in create buf callback
 end
 
-module MakePointBidir
-  (SA : SETUP)
-  (SB : SETUP with type q_t = SA.q_t) =
+module MakeBidirHelper
+  (SA : BASE)
+  (SB : BASE with type q_t = SA.q_t) =
 struct
   type q_t = SA.q_t
-  type a_t = SA.q_t
-  type b_t = SA.q_t
-
   module Connect = struct
     module SA = MakeTPSetup(SA)
     module SB = MakeTPSetup(SB)
   end
-
   module Bidir = TP.MakeBidir (Connect)
+end
+
+module MakeBidir
+  (SA : SETUP)
+  (SB : SETUP with type q_t = SA.q_t) =
+struct
+  include MakeBidirHelper (SA) (SB)
+  type a_t = SA.target_t
+  type b_t = SB.target_t
+
+  let planner stop (a, b) =
+    let ta = Connect.SA.create_callback (SA.create_callback a) in
+    let tb = Connect.SB.create_callback (SB.create_callback b) in
+      Bidir.planner stop (ta, tb)
+end
+
+module MakePointBidir
+  (SA : BASE)
+  (SB : BASE with type q_t = SA.q_t) =
+struct
+  include MakeBidirHelper (SA) (SB)
+  type a_t = SA.q_t
+  type b_t = SA.q_t
 
   let planner stop (a, b) =
     let ta = Connect.SA.create_point a in
 end
 
 module MakeSamplerBidir
-  (SA : SETUP)
-  (SB : SETUP with type q_t = SA.q_t) =
+  (SA : BASE)
+  (SB : BASE with type q_t = SA.q_t) =
 struct
-  type q_t = SA.q_t
+  include MakeBidirHelper (SA) (SB)
   type a_t = SA.q_t PaplPlanner.region_t
   type b_t = SA.q_t PaplPlanner.region_t
 
-  module Connect = struct
-    module SA = MakeTPSetup(SA)
-    module SB = MakeTPSetup(SB)
-  end
-
-  module Bidir = TP.MakeBidir (Connect)
-
   let planner stop (a, b) =
     let ta = Connect.SA.create_region a in
     let tb = Connect.SB.create_region b in
 end
 
 module BruteForce = struct
-  module type SETUP = sig
+  module type BASE = sig
     type q_t
     include PaplTreeIndex.BruteForce.RRT.SETUP with type q_t := q_t
 
     val sampler : q_t option PaplSampler.t
   end
 
-  module MakeRRTSetup (S : SETUP) = struct
+  module type SETUP = sig
+    include BASE
+    type target_t
+
+    val create_callback : target_t -> (q_t -> unit) -> callback_t
+  end
+
+  module MakeRRTSetupBase (S : BASE) = struct
     include S
     type value_t = q_t
 
     module Index = PaplTreeIndex.BruteForce.RRT.Make(S)
   end
 
-  module MakePointBidir
+  module MakeRRTSetup (S : SETUP) = struct
+    include MakeRRTSetupBase (S)
+    type target_t = S.target_t
+
+    let create_callback target buf =
+      let add_root q = let _ = Index.add_root buf (make_value q) in ()
+      in S.create_callback target add_root
+  end
+
+  module MakeBidir
     (SA : SETUP)
     (SB : SETUP with type q_t = SA.q_t) =
-    MakePointBidir
+    MakeBidir
       (MakeRRTSetup(SA))
       (MakeRRTSetup(SB))
 
+  module MakePointBidir
+    (SA : BASE)
+    (SB : BASE with type q_t = SA.q_t) =
+    MakePointBidir
+      (MakeRRTSetupBase(SA))
+      (MakeRRTSetupBase(SB))
+
   module MakeSamplerBidir
-    (SA : SETUP)
-    (SB : SETUP with type q_t = SA.q_t) =
+    (SA : BASE)
+    (SB : BASE with type q_t = SA.q_t) =
     MakeSamplerBidir
-      (MakeRRTSetup(SA))
-      (MakeRRTSetup(SB))
+      (MakeRRTSetupBase(SA))
+      (MakeRRTSetupBase(SB))
 end

File src/PaplRRT.mli

View file
  • Ignore whitespace
 (** RRT planners *)
 
-module type SETUP = sig
+type callback_t = unit -> unit
+
+module type BASE = sig
   type q_t
   type value_t
 
   val get_q : value_t -> q_t
 end
 
-module MakePointBidir
+module type SETUP = sig
+  include BASE
+  type target_t
+
+  val create_callback : target_t -> Index.t -> callback_t
+end
+
+module MakeBidir
   (SA : SETUP)
   (SB : SETUP with type q_t = SA.q_t) :
+  PaplPlanner.BIDIR
+  with type q_t = SA.q_t
+  and type a_t = SA.target_t
+  and type b_t = SB.target_t
+
+module MakePointBidir
+  (SA : BASE)
+  (SB : BASE with type q_t = SA.q_t) :
   PaplPlanner.POINT_BIDIR
   with type q_t = SA.q_t
 
 module MakeSamplerBidir
-  (SA : SETUP)
-  (SB : SETUP with type q_t = SA.q_t) :
+  (SA : BASE)
+  (SB : BASE with type q_t = SA.q_t) :
   PaplPlanner.SAMPLER_BIDIR
   with type q_t = SA.q_t
 
 module BruteForce : sig
-  module type SETUP = sig
+  module type BASE = sig
     type q_t
     include PaplTreeIndex.BruteForce.RRT.SETUP with type q_t := q_t
 
     val sampler : q_t option PaplSampler.t
   end
 
-  module MakePointBidir
+  module type SETUP = sig
+    include BASE
+    type target_t
+
+    val create_callback : target_t -> (q_t -> unit) -> callback_t
+  end
+
+  module MakeBidir
     (SA : SETUP)
     (SB : SETUP with type q_t = SA.q_t) :
+    PaplPlanner.BIDIR
+    with type q_t = SA.q_t
+    and type a_t = SA.target_t
+    and type b_t = SB.target_t
+
+  module MakePointBidir
+    (SA : BASE)
+    (SB : BASE with type q_t = SA.q_t) :
     PaplPlanner.POINT_BIDIR
     with type q_t = SA.q_t
 
   module MakeSamplerBidir
-    (SA : SETUP)
-    (SB : SETUP with type q_t = SA.q_t) :
+    (SA : BASE)
+    (SB : BASE with type q_t = SA.q_t) :
     PaplPlanner.SAMPLER_BIDIR
     with type q_t = SA.q_t
 end