Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 886d5df77b2a6cdabceaa6d82319a6e656cee2b9
|
From: Jérémie D. <Ba...@us...> - 2011-04-17 16:25:42
|
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 886d5df77b2a6cdabceaa6d82319a6e656cee2b9 (commit)
from ef0c23665393de15b54be73047a9d65214d3c23d (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 886d5df77b2a6cdabceaa6d82319a6e656cee2b9
Author: Jérémie Dimino <je...@di...>
Date: Sun Apr 17 18:25:12 2011 +0200
[info] keep the origin while moving
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/interfaces/krobot_interface_planner.obus b/info/control2011/src/interfaces/krobot_interface_planner.obus
index 06c722a..ac8f5d7 100644
--- a/info/control2011/src/interfaces/krobot_interface_planner.obus
+++ b/info/control2011/src/interfaces/krobot_interface_planner.obus
@@ -10,6 +10,9 @@
(** Interface for the planner. *)
interface fr.krobot.Planner {
+ property_r origin : ((double * double) * (double * double))
+ (** The origin of the trajectory, with the initial vector. *)
+
property_rw vertices : (double * double) array
(** Property holding the current list of vertices for the planned
trajectory. *)
diff --git a/info/control2011/src/lib/krobot_planner.ml b/info/control2011/src/lib/krobot_planner.ml
index 918b836..c1fc2d1 100644
--- a/info/control2011/src/lib/krobot_planner.ml
+++ b/info/control2011/src/lib/krobot_planner.ml
@@ -17,6 +17,11 @@ type t = Krobot_bus.t
let proxy bus =
OBus_proxy.make (OBus_peer.make (Krobot_bus.to_bus bus) "fr.krobot.Service.Planner") ["fr"; "krobot"; "Planner"]
+let origin bus =
+ OBus_property.map_r
+ (fun ((x, y), (vx, vy)) -> ({ x; y }, ({ vx; vy })))
+ (OBus_property.make p_origin (proxy bus))
+
let vertices bus =
OBus_property.map_rw
(List.map (fun (x, y) -> { x; y }))
diff --git a/info/control2011/src/lib/krobot_planner.mli b/info/control2011/src/lib/krobot_planner.mli
index 2287ee7..077956e 100644
--- a/info/control2011/src/lib/krobot_planner.mli
+++ b/info/control2011/src/lib/krobot_planner.mli
@@ -10,6 +10,9 @@
type t = Krobot_bus.t
(** Type of the planner. *)
+val origin : t -> (Krobot_geom.vertice * Krobot_geom.vector, [ `readable ]) OBus_property.t
+ (** The origin of the trajectory. *)
+
val vertices : t -> (Krobot_geom.vertice list, [ `readable | `writable ]) OBus_property.t
(** The property holding the current trajectory. *)
diff --git a/info/control2011/src/services/krobot_service_planner.ml b/info/control2011/src/services/krobot_service_planner.ml
index 3626e0d..cdda7e5 100644
--- a/info/control2011/src/services/krobot_service_planner.ml
+++ b/info/control2011/src/services/krobot_service_planner.ml
@@ -39,6 +39,14 @@ type planner = {
obus : planner OBus_object.t;
(* The D-Bus object attached to this planner. *)
+ origin : (vertice * vector) signal;
+ (* If the robot is moving, this is the origin of the current
+ trajectory with the initial direction vector, otherwise it is the
+ current position with the current direction vector. *)
+
+ set_origin : (vertice * vector) -> unit;
+ (* Set the origin of the trajectory. *)
+
vertices : vertice list signal;
(* The list of vertices for the trajectory. *)
@@ -155,6 +163,9 @@ let go planner rotation_speed rotation_acceleration moving_speed moving_accelera
(match S.value planner.vertices with
| _ :: l -> planner.set_vertices l
| [] -> ());
+ planner.set_origin (planner.position,
+ { vx = cos planner.orientation;
+ vy = sin planner.orientation });
loop ()
end else
@@ -188,6 +199,9 @@ let create bus =
OBus_object.make
~interfaces:[
Krobot_interface_planner.Fr_krobot_Planner.make {
+ p_origin =
+ (fun obj ->
+ S.map (fun ({ x; y }, { vx; vy }) -> ((x, y), (vx, vy))) (OBus_object.get obj).origin);
p_vertices =
((fun obj ->
S.map (List.map (fun { x; y } -> (x, y))) (OBus_object.get obj).vertices),
@@ -215,11 +229,14 @@ let create bus =
]
["fr"; "krobot"; "Planner"]
in
+ let origin, set_origin = S.create ({ x = 0.; y = 0. }, { vx = 1.; vy = 0. }) in
let vertices, set_vertices = S.create [] in
let moving, set_moving = S.create false in
let planner = {
bus;
obus = obus_object;
+ origin;
+ set_origin;
vertices;
set_vertices;
moving;
@@ -238,7 +255,11 @@ let create bus =
match frame with
| Odometry(x, y, theta) ->
planner.position <- { x; y };
- planner.orientation <- math_mod_float theta (2. *. pi)
+ planner.orientation <- math_mod_float theta (2. *. pi);
+ if not (S.value planner.moving) then
+ planner.set_origin (planner.position,
+ { vx = cos planner.orientation;
+ vy = sin planner.orientation })
| Motor_status(m1, m2, m3, m4) ->
planner.motors_moving <- m1 || m2
| _ ->
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 1739473..dc3ce69 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -208,6 +208,9 @@ module Board = struct
mutable beacon : beacon;
(* The state of the beacon. *)
+ origin : (vertice * vector) signal;
+ (* The origin of the trajectory. *)
+
vertices : vertice list signal;
(* The current trajectory. *)
@@ -415,9 +418,11 @@ module Board = struct
Cairo.stroke ctx
end;
+ let o, v = S.value board.origin in
+
(* Draw points. *)
Cairo.set_source_rgb ctx 255. 255. 0.;
- Cairo.move_to ctx board.state.pos.x board.state.pos.y;
+ Cairo.move_to ctx o.x o.y;
List.iter (fun { x; y } -> Cairo.line_to ctx x y) (S.value board.vertices);
Cairo.stroke ctx;
@@ -432,9 +437,7 @@ module Board = struct
Cairo.line_to ctx x y
done;
Cairo.stroke ctx)
- { vx = cos board.state.theta; vy = sin board.state.theta }
- (board.state.pos :: S.value board.vertices)
- ();
+ v (o :: S.value board.vertices) ();
let ctx = Cairo_lablgtk.create board.ui#scene#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
@@ -467,61 +470,9 @@ module Board = struct
let tolerance = board.ui#tolerance#adjustment#value in
ignore_result (Krobot_planner.simplify board.bus tolerance)
-(* let bezier_vertices q r v1 v2 =
- (* Create the bezier curve. *)
- let curve = Bezier.of_vertices q (translate q v1) (translate r v2) r in
-
- (* Create vertices. *)
- let vertices = Array.create 101 origin in
- for i = 0 to 100 do
- vertices.(i) <- Bezier.vertice curve (float i /. 100.)
- done;
-
- vertices
- in
-
- (* Compute cubic bezier curves. *)
- let rec loop = function
- | p :: (q :: r :: s :: _ as rest) ->
- (* Computes tangents with a length that is half of the
- minimum length of the adjacent segments. *)
- let _, v1 = tangents p q r and v2, _ = tangents q r s in
- let v1 = v1 *| (min (distance p q) (distance q r) /. 2.)
- and v2 = v2 *| (min (distance q r) (distance r s) /. 2.) in
- bezier_vertices q r v1 v2 :: loop rest
- | [p; q; r] ->
- let _, v1 = tangents p q r and v2 = vector r q /| distance q r in
- let v1 = v1 *| (min (distance p q) (distance q r) /. 2.)
- and v2 = v2 *| (distance q r /. 2.) in
- [bezier_vertices q r v1 v2]
- | _ ->
- []
- in
- begin
- match board.points with
- | r :: s :: _ ->
- let q = board.state.pos in
- let v1 = { vx = cos board.state.theta; vy = sin board.state.theta }
- and v2, _ = tangents q r s in
- let v1 = v1 *| (distance q r /. 2.)
- and v2 = v2 *| (distance q r /. 2.) in
- board.bezier <- bezier_vertices q r v1 v2 :: loop (q :: board.points);
- | [r] ->
- let q = board.state.pos in
- let v1 = { vx = cos board.state.theta; vy = sin board.state.theta }
- and v2 = vector r q /| distance q r in
- let v1 = v1 *| (distance q r /. 2.)
- and v2 = v2 *| (distance q r /. 2.) in
- board.bezier <- bezier_vertices q r v1 v2 :: loop (q :: board.points);
- | [] ->
- board.bezier <- []
- end;
-
- queue_draw board
-*)
-
let create bus ui =
- lwt vertices = OBus_property.monitor (Krobot_planner.vertices bus) in
+ lwt origin = OBus_property.monitor (Krobot_planner.origin bus)
+ and vertices = OBus_property.monitor (Krobot_planner.vertices bus) in
let board ={
bus;
ui;
@@ -531,6 +482,7 @@ module Board = struct
theta = -0.5 *. pi
};
beacon = { xbeacon = 1.; ybeacon = 1.; valid = false };
+ origin;
vertices;
events = [];
statusbar_context = ui#statusbar#new_context "";
hooks/post-receive
--
krobot
|
