Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. c5fbd9bd2e6001ab07cbe63de4a8859f91ed75c8
|
From: Jérémie D. <Ba...@us...> - 2011-05-26 19:38:34
|
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 c5fbd9bd2e6001ab07cbe63de4a8859f91ed75c8 (commit)
from 398467100644cc193109027565a75a116aff0151 (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 c5fbd9bd2e6001ab07cbe63de4a8859f91ed75c8
Author: Jérémie Dimino <je...@di...>
Date: Thu May 26 21:37:26 2011 +0200
[info] start of path planning
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/_oasis b/info/control2011/_oasis
index eea9438..90561d3 100644
--- a/info/control2011/_oasis
+++ b/info/control2011/_oasis
@@ -153,7 +153,7 @@ Executable "krobot-planner"
Install: true
CompiledObject: best
MainIs: krobot_planner.ml
- BuildDepends: krobot, lwt.syntax
+ BuildDepends: krobot, lwt.syntax, ocamlgraph
Executable "krobot-objects"
Path: src/tools
diff --git a/info/control2011/_tags b/info/control2011/_tags
index 8d7cff0..20c227a 100644
--- a/info/control2011/_tags
+++ b/info/control2011/_tags
@@ -2,7 +2,7 @@
<src/interfaces/*.ml>: -syntax_camlp4o
# OASIS_START
-# DO NOT EDIT (digest: 727ec33b4060953b9ebbde97c6c14363)
+# DO NOT EDIT (digest: 1d3ff08546f26990d32be2616f7495bc)
# Library krobot
"src/lib": include
"src/lib/krobot.cmxs": use_krobot
@@ -109,9 +109,11 @@
<examples/*.ml{,i}>: pkg_lwt.react
# Executable krobot-planner
<src/tools/krobot_planner.{native,byte}>: use_krobot
+<src/tools/krobot_planner.{native,byte}>: pkg_ocamlgraph
<src/tools/krobot_planner.{native,byte}>: pkg_lwt.unix
<src/tools/krobot_planner.{native,byte}>: pkg_lwt.syntax
<src/tools/krobot_planner.{native,byte}>: pkg_lwt.react
+<src/tools/*.ml{,i}>: pkg_ocamlgraph
# Executable krobot-record
<src/tools/krobot_record.{native,byte}>: use_krobot
<src/tools/krobot_record.{native,byte}>: pkg_lwt.unix
diff --git a/info/control2011/setup.ml b/info/control2011/setup.ml
index c202ad2..33d9d54 100644
--- a/info/control2011/setup.ml
+++ b/info/control2011/setup.ml
@@ -1,7 +1,7 @@
(* setup.ml generated for the first time by OASIS v0.2.0~alpha1 *)
(* OASIS_START *)
-(* DO NOT EDIT (digest: 79727b7548ae5f100d31519956433b0b) *)
+(* DO NOT EDIT (digest: b50f9201082ebd643759d008620ee0bf) *)
(*
Regenerated by OASIS v0.2.0
Visit http://oasis.forge.ocamlcore.org for more information and
@@ -5575,7 +5575,8 @@ let setup_t =
bs_build_depends =
[
InternalLibrary "krobot";
- FindlibPackage ("lwt.syntax", None)
+ FindlibPackage ("lwt.syntax", None);
+ FindlibPackage ("ocamlgraph", None)
];
bs_build_tools = [ExternalTool "ocamlbuild"];
bs_c_sources = [];
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index 101151b..3d423b8 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -32,6 +32,7 @@ type message =
| Trajectory_go of float * float * float * float
| Trajectory_stop
| Trajectory_moving of bool
+ | Trajectory_find_path
| Objects of vertice list
type t = {
@@ -102,6 +103,8 @@ let string_of_message = function
sprintf
"Trajectory_moving %B"
b
+ | Trajectory_find_path ->
+ "Trajectory_find_path"
| Objects objects ->
sprintf
"Objects [%s]"
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index 28a37a2..dd7f2e3 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -50,6 +50,8 @@ type message =
(** Stop the current trajectory. *)
| Trajectory_moving of bool
(** Is the robot following a trajectory ? *)
+ | Trajectory_find_path
+ (** Find a path avoiding objects. *)
(** Objects *)
diff --git a/info/control2011/src/lib/krobot_config.ml b/info/control2011/src/lib/krobot_config.ml
index 761a334..9bcbd9c 100644
--- a/info/control2011/src/lib/krobot_config.ml
+++ b/info/control2011/src/lib/krobot_config.ml
@@ -14,3 +14,4 @@ let wheels_diameter = 0.098
let wheels_distance = 0.150
let wheels_position = robot_size /. 2.
let rotary_beacon_index_pos = (4. *. atan 1.) /. 2. (* left side *)
+let object_radius = 0.1
diff --git a/info/control2011/src/lib/krobot_config.mli b/info/control2011/src/lib/krobot_config.mli
index 266daaf..fc140a8 100644
--- a/info/control2011/src/lib/krobot_config.mli
+++ b/info/control2011/src/lib/krobot_config.mli
@@ -31,3 +31,6 @@ val wheels_position : float
val rotary_beacon_index_pos : float
(** The angle of the rotary beacon index angle with respect to the
robot's front *)
+
+val object_radius : float
+ (** Radius of objects. *)
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index b15a336..d6d1796 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -59,6 +59,111 @@ type planner = {
}
(* +-----------------------------------------------------------------+
+ | Motion planning |
+ +-----------------------------------------------------------------+ *)
+
+module Vertice_set = Set.Make(struct type t = vertice let compare = compare end)
+module Vertice_map = Map.Make(struct type t = vertice let compare = compare end)
+
+module Dijkstra =
+ Graph.Path.Dijkstra
+ (struct
+ type t = Vertice_set.t Vertice_map.t
+
+ module V = struct
+ type t = vertice
+ let compare = Pervasives.compare
+ let hash = Hashtbl.hash
+ let equal = (=)
+ end
+
+ module E = struct
+ type t = vertice * vertice
+ type label = float
+ let label (a, b) = distance a b
+ let dst (a, b) = b
+ end
+
+ let iter_succ_e f graph v =
+ Vertice_set.iter (fun v' -> f (v, v')) (Vertice_map.find v graph)
+ end)
+ (struct
+ type label = float
+ type t = float
+ let weight d = d
+ let compare a b = compare a b
+ let add a b = a +. b
+ let zero = 0.
+ end)
+
+let sqr x = x *. x
+
+let dist = object_radius +. robot_size /. 2.
+
+(* Test whether there is an intersection between the line (va, vb) and
+ one of the objects. *)
+let rec intersection va vb objects =
+ match objects with
+ | [] ->
+ false
+ | vc :: rest ->
+ (* Compute coefficients of the polynomial. *)
+ let a = sqr (distance va vb)
+ and b = -2. *. prod (vector va vc) (vector va vb)
+ and c = sqr (distance va vc) -. sqr dist in
+ let delta = sqr b -. 4. *. a *. c in
+ if delta < 0. then
+ intersection va vb rest
+ else
+ let k1 = (-. b -. sqrt delta) /. (2. *. a)
+ and k2 = (-. b +. sqrt delta) /. (2. *. a) in
+ if (k1 >= 0. && k1 <= 1.) || (k2 >= 0. && k2 <= 1.) then
+ true
+ else
+ intersection va vb rest
+
+let find_path planner src dst =
+ (* Build bounding boxes. *)
+ let r = object_radius +. robot_size in
+ let vertices =
+ List.fold_left
+ (fun set obj ->
+ Vertice_set.add { x = obj.x; y = obj.y -. r }
+ (Vertice_set.add { x = obj.x +. r; y = obj.y }
+ (Vertice_set.add { x = obj.x; y = obj.y +. r }
+ (Vertice_set.add { x = obj.x -. r; y = obj.y }
+ set))))
+ Vertice_set.empty planner.objects
+ in
+ (* Add the source and the destination. *)
+ let vertices = Vertice_set.add src (Vertice_set.add dst vertices) in
+ (* Build the graph. *)
+ let graph =
+ Vertice_set.fold
+ (fun va map ->
+ let successors =
+ Vertice_set.fold
+ (fun vb set ->
+ if va <> vb then
+ if intersection va vb planner.objects then
+ set
+ else
+ Vertice_set.add vb set
+ else
+ set)
+ vertices Vertice_set.empty
+ in
+ Vertice_map.add va successors map)
+ vertices Vertice_map.empty
+ in
+ try
+ (* Compute the shortest path. *)
+ let path, weight = Dijkstra.shortest_path graph src dst in
+ List.map (fun (a, b) -> b) path
+ with Not_found ->
+ [dst]
+
+(* +-----------------------------------------------------------------+
| Primitives |
+-----------------------------------------------------------------+ *)
@@ -292,6 +397,15 @@ let handle_message planner (timestamp, message) =
set_vertices planner [];
ignore (Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_stop(1.0,0.0)))
+ | Trajectory_find_path ->
+ if not planner.moving then begin
+ match planner.vertices with
+ | v :: _ ->
+ set_vertices planner (find_path planner planner.position v)
+ | _ ->
+ ()
+ end
+
| Objects l ->
planner.objects <- l
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 98a36fc..ea98946 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -531,6 +531,13 @@ lwt () =
false));
ignore
+ (ui#button_find#event#connect#button_release
+ (fun ev ->
+ if GdkEvent.Button.button ev = 1 then
+ ignore (Krobot_bus.send viewer.bus (Unix.gettimeofday (), Trajectory_find_path));
+ false));
+
+ ignore
(ui#button_go#event#connect#button_release
(fun ev ->
if GdkEvent.Button.button ev = 1 then
diff --git a/info/control2011/src/tools/krobot_viewer_ui.glade b/info/control2011/src/tools/krobot_viewer_ui.glade
index b4facd3..351e0c4 100644
--- a/info/control2011/src/tools/krobot_viewer_ui.glade
+++ b/info/control2011/src/tools/krobot_viewer_ui.glade
@@ -200,7 +200,7 @@
<child>
<widget class="GtkTable" id="table2">
<property name="visible">True</property>
- <property name="n_rows">3</property>
+ <property name="n_rows">4</property>
<property name="n_columns">2</property>
<child>
<widget class="GtkButton" id="button_clear">
@@ -230,8 +230,8 @@
<property name="receives_default">True</property>
</widget>
<packing>
- <property name="top_attach">1</property>
- <property name="bottom_attach">2</property>
+ <property name="top_attach">2</property>
+ <property name="bottom_attach">3</property>
</packing>
</child>
<child>
@@ -244,8 +244,8 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">1</property>
- <property name="bottom_attach">2</property>
+ <property name="top_attach">2</property>
+ <property name="bottom_attach">3</property>
</packing>
</child>
<child>
@@ -256,8 +256,8 @@
<property name="receives_default">True</property>
</widget>
<packing>
- <property name="top_attach">2</property>
- <property name="bottom_attach">3</property>
+ <property name="top_attach">3</property>
+ <property name="bottom_attach">4</property>
</packing>
</child>
<child>
@@ -270,8 +270,23 @@
<packing>
<property name="left_attach">1</property>
<property name="right_attach">2</property>
- <property name="top_attach">2</property>
- <property name="bottom_attach">3</property>
+ <property name="top_attach">3</property>
+ <property name="bottom_attach">4</property>
+ </packing>
+ </child>
+ <child>
+ <placeholder/>
+ </child>
+ <child>
+ <widget class="GtkButton" id="button_find">
+ <property name="label" translatable="yes">Find a path</property>
+ <property name="visible">True</property>
+ <property name="can_focus">True</property>
+ <property name="receives_default">True</property>
+ </widget>
+ <packing>
+ <property name="top_attach">1</property>
+ <property name="bottom_attach">2</property>
</packing>
</child>
</widget>
hooks/post-receive
--
krobot
|
