krobot-commits

[Pierre C. <Ba...@us...>]

This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".

The branch, master has been updated
       via  ce3ff750e287178ea2034d0a683371562d679e9e (commit)
       via  33c5cf6e79111ec89d78e7e4b1659548665511e7 (commit)
      from  ea4f473d3e5947d777e4cecee84681d00961496c (commit)

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- Log -----------------------------------------------------------------
commit ce3ff750e287178ea2034d0a683371562d679e9e
Author: Pierre Chambart <pie...@oc...>
Date:   Thu Apr 11 02:52:55 2013 +0200

    Change krobot_urg dump format

commit 33c5cf6e79111ec89d78e7e4b1659548665511e7
Author: Pierre Chambart <pie...@oc...>
Date:   Thu Apr 11 02:48:21 2013 +0200

    Change the transmition format for urg messages, add to the viewer

-----------------------------------------------------------------------

Changes:
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index 34595f3..387f972 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -41,7 +41,8 @@ type message =
   | Set_fake_beacons of vertice option * vertice option
   | Collisions of Bezier.curve * (float * (vertice * float) option) list
   | Coins of vertice list
-  | Urg of int array
+  | Urg of vertice array
+  | Urg_lines of (vertice*vertice) array
   | Beacon_raw of (int * int * int * int * int * int
       * int * int * int * int * int)
 
@@ -151,6 +152,8 @@ let string_of_message = function
               l))
   | Urg distances ->
       sprintf "Urg (many_points...)"
+  | Urg_lines lines ->
+      sprintf "Urg_lines (many_lines...)"
   | Beacon_raw _ ->
       sprintf "Raw beacon packet"
 
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index ad33c96..b04aac3 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -87,7 +87,8 @@ type message =
   | Coins of vertice list
 
   (** distances mesured by the URG (in millimeters) *)
-  | Urg of int array
+  | Urg of vertice array
+  | Urg_lines of (vertice*vertice) array
 
   | Beacon_raw of (int * int * int * int * int * int
       * int * int * int * int * int)
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index 43189d7..127b63e 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -119,6 +119,7 @@ let initial_coins = []
     (* 2.55, 0.3; *) ]
 *)
 
+let urg_position = { x = 0.19; y = 0. }
 
 let urg_angles =
   [|-2.356194490192344836998472601408; -2.350058567040802071090865865699;
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
index 6770e8b..159f59f 100644
--- a/info/control2011/src/lib/krobot_config.mli
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -53,5 +53,7 @@ val fixed_obstacles : Krobot_geom.obj list
 
 val initial_coins : Krobot_geom.vertice list
 
+val urg_position : Krobot_geom.vertice
+
 val urg_angles : float array
 (* angles (in radiant) for each index of urg messages *)
diff --git a/info/control2011/src/tools/krobot_urg.ml b/info/control2011/src/tools/krobot_urg.ml
index 6b1acaf..aecb465 100644
--- a/info/control2011/src/tools/krobot_urg.ml
+++ b/info/control2011/src/tools/krobot_urg.ml
@@ -14,6 +14,7 @@ open Lwt_react
 open Lwt_preemptive
 open Krobot_bus
 open Krobot_message
+open Krobot_geom
 
 let section = Lwt_log.Section.make "krobot(urg)"
 
@@ -21,21 +22,32 @@ let section = Lwt_log.Section.make "krobot(urg)"
    | read/send loop                                                  |
    +-----------------------------------------------------------------+ *)
 
-let to_array (b:Urg.point_data) =
+let scale = 0.001
+
+let convert_pos dist angle =
+  let x = float dist *. cos angle *. scale +. Krobot_config.urg_position.x in
+  let y = float dist *. sin angle *. scale +. Krobot_config.urg_position.y in
+  (x), (-. y) (* the urg is top down -> y is reversed *)
+
+let convert (b:Urg.point_data) =
   let dim = Bigarray.Array1.dim b in
-  let a = Array.create dim 0 in
+  let l = ref [] in
   for i = 0 to dim - 1 do
-    a.(i) <- Nativeint.to_int b.{i};
+    let data = Nativeint.to_int b.{i} in
+    if data > 0
+    then
+      let angle = Krobot_config.urg_angles.(i) in
+      let (x,y) = convert_pos data angle in
+      l := {x;y} :: !l
   done;
-  a
+  Array.of_list !l
 
 let loop bus urg =
   let rec aux () =
     lwt _ = Lwt_preemptive.detach Urg_simple.get urg in
     let time = Unix.gettimeofday () in
-    let msg = Urg (to_array urg.Urg_simple.data) in
+    let msg = Urg (convert urg.Urg_simple.data) in
     lwt () = Krobot_bus.send bus (time, msg) in
-    lwt () = Lwt_log.info_f "send things" in
     lwt () = Lwt_unix.sleep 0.01 in
     aux () in
   aux ()
@@ -57,31 +69,21 @@ let handle_message (timestamp, message) =
     | _ ->
         ()
 
-let min_val = 10
-
-let less a b =
-  if a < min_val
-  then false
-  else if b < min_val
-  then true
-  else a < b
-
-let extrema a cmp =
-  let m = ref a.(0) in
-  let pos = ref 0 in
-  for i = 0 to Array.length a - 1 do
-    let v = a.(i) in
-    if cmp v !m
-    then (m := v; pos := i)
-  done;
-  !m, !pos
+(* let print_pos l = *)
+(*   Format.printf "[@[<1>@ "; *)
+(*   Array.iter (fun {x;y} -> Format.printf "(%f,%f);@ " x y) l; *)
+(*   Format.printf "@]]@." *)
+
+let print_pos ts l =
+  Printf.printf "%f " ts;
+  Array.iter (fun {x;y} -> Format.printf "%f %f " x y) l;
+  Printf.printf "\n%!"
 
 let handle_listener (timestamp, message) =
   match message with
     | Urg data ->
-      let min, pos_min = extrema data less in
-      let rad_min = Krobot_config.urg_angles.(pos_min) in
-      Lwt_io.printf "%i %f\n%!" min rad_min
+      print_pos timestamp data;
+      return ()
     | _ -> Lwt.return ()
 
 (* +-----------------------------------------------------------------+
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 8ccb7b0..564a27c 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -61,6 +61,9 @@ type viewer = {
 
   mutable collisions : (Bezier.curve * (float * (vertice * float) option) list) option;
   (* A curve and a list of: [(curve_parameter, (center, radius))] *)
+
+  mutable urg : vertice array;
+  mutable urg_lines : (vertice*vertice) array;
 }
 
 (* +-----------------------------------------------------------------+
@@ -353,6 +356,25 @@ let draw viewer =
   draw_beacon b1;
   draw_beacon b2;
 
+  let draw_urg a =
+    Cairo.set_source_rgba ctx 0.5 0.5 0.5 0.5;
+    let aux {x;y} =
+      Cairo.arc ctx x y 0.01 0. (2. *. pi);
+      Cairo.fill ctx
+    in
+    Array.iter aux a in
+  draw_urg viewer.urg;
+
+  let draw_urg_lines a =
+    Cairo.set_source_rgba ctx 1. 0.5 1. 0.5;
+    let aux ({x=x1;y=y1},{x=x2;y=y2}) =
+      Cairo.move_to ctx x1 y1;
+      Cairo.line_to ctx x2 y2;
+      Cairo.stroke ctx
+    in
+    Array.iter aux a in
+  draw_urg_lines viewer.urg_lines;
+
   (* Draw the path of the VM if any or the path of the planner if the
      VM is not following a trajectory. *)
   let path =
@@ -474,6 +496,37 @@ let set_beacons viewer x y =
              Krobot_bus.Set_fake_beacons (b1, b2)))
 
 (* +-----------------------------------------------------------------+
+   | Urg handling                                                    |
+   +-----------------------------------------------------------------+ *)
+
+let convert_pos dist angle =
+  let x = float dist *. cos angle *. 0.001 in
+  let y = float dist *. sin angle *. 0.001 in
+  { x ; y }
+
+let project_urg viewer pos =
+  (* TODO: put the real urg position rather than the robot position *)
+  let theta = viewer.state.theta in
+  let rot = rot_mat theta in
+  let f { x;y } =
+    let urg_pos = [| x; y; 1. |] in
+    let urg_pos = mult rot urg_pos in
+    let state_pos = Krobot_geom.translate viewer.state.pos
+        { vx = urg_pos.(0); vy = urg_pos.(1) } in
+    state_pos
+  in
+  Array.map f pos
+
+let project_urg_lines viewer lines =
+  let rot = rot_mat viewer.state.theta in
+  let f v =
+    let v = [|v.x;v.y;1.|] in
+    let v = mult rot v in
+    Krobot_geom.translate viewer.state.pos { vx = v.(0); vy = v.(1) }
+  in
+  Array.map (fun (v1,v2) -> f v1,f v2) lines
+
+(* +-----------------------------------------------------------------+
    | Message handling                                                |
    +-----------------------------------------------------------------+ *)
 
@@ -605,6 +658,12 @@ let handle_message viewer (timestamp, message) =
       viewer.collisions <- Some (curve, l);
       queue_draw viewer
 
+    | Urg dist ->
+      viewer.urg <- project_urg viewer dist
+
+    | Urg_lines lines ->
+      viewer.urg_lines <- project_urg_lines viewer lines
+
     | _ ->
         ()
 
@@ -657,6 +716,8 @@ lwt () =
     motor_status = (false, false, false, false);
     coins = Krobot_config.initial_coins;
     collisions = None;
+    urg = [||];
+    urg_lines = [||];
   } in
 
   (* Handle messages. *)


hooks/post-receive
-- 
krobot