Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 1626abcabb9345aa8858641cc53da6a41b6d7033
|
From: Jérémie D. <Ba...@us...> - 2011-04-20 21:10:14
|
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 1626abcabb9345aa8858641cc53da6a41b6d7033 (commit)
via a93c9138dcd65b01bb4b3432486557db62bb6847 (commit)
via 0dc2179cc162ec5aefa4140793d9246f0d50ea6e (commit)
from a23abb45064d154e262a1e82a3112976f082950e (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 1626abcabb9345aa8858641cc53da6a41b6d7033
Author: Jérémie Dimino <je...@di...>
Date: Wed Apr 20 23:08:43 2011 +0200
[krobot_planner] clear the trajectory at the end of one
commit a93c9138dcd65b01bb4b3432486557db62bb6847
Author: Jérémie Dimino <je...@di...>
Date: Wed Apr 20 23:01:50 2011 +0200
[krobot_planner] do not remove the origin of the trajectory too early
commit 0dc2179cc162ec5aefa4140793d9246f0d50ea6e
Author: Jérémie Dimino <je...@di...>
Date: Wed Apr 20 22:56:49 2011 +0200
[info] keep bezier curves in sync between the planner and the viewer
-----------------------------------------------------------------------
Changes:
diff --git a/info/control2011/src/lib/krobot_bus.ml b/info/control2011/src/lib/krobot_bus.ml
index 8633395..cf22f9d 100644
--- a/info/control2011/src/lib/krobot_bus.ml
+++ b/info/control2011/src/lib/krobot_bus.ml
@@ -25,8 +25,7 @@ type message =
| Log of string
| Send
| Kill of string
- | Trajectory_origin of vertice * vector
- | Trajectory_vertices of vertice list
+ | Trajectory_vertices of vertice list * (vertice * vertice * vertice * vertice) list
| Trajectory_set_vertices of vertice list
| Trajectory_add_vertice of vertice
| Trajectory_simplify of float
@@ -67,15 +66,19 @@ let string_of_message = function
sprintf
"Kill %S"
name
- | Trajectory_origin(o, v) ->
+ | Trajectory_vertices(vertices, curves) ->
sprintf
- "Trajectory_origin(%s, %s)"
- (string_of_vertice o)
- (string_of_vector v)
- | Trajectory_vertices l ->
- sprintf
- "Trajectory_vertices [%s]"
- (String.concat "; " (List.map string_of_vertice l))
+ "Trajectory_vertices([%s], [%s])"
+ (String.concat "; " (List.map string_of_vertice vertices))
+ (String.concat "; " (List.map
+ (fun (p, q, r, s) ->
+ Printf.sprintf
+ "(%s, %s, %s, %s)"
+ (string_of_vertice p)
+ (string_of_vertice q)
+ (string_of_vertice r)
+ (string_of_vertice s))
+ curves))
| Trajectory_set_vertices l ->
sprintf
"Trajectory_set_vertices [%s]"
diff --git a/info/control2011/src/lib/krobot_bus.mli b/info/control2011/src/lib/krobot_bus.mli
index fb1333a..c453e26 100644
--- a/info/control2011/src/lib/krobot_bus.mli
+++ b/info/control2011/src/lib/krobot_bus.mli
@@ -34,11 +34,9 @@ type message =
(** Trajectory messages. *)
- | Trajectory_origin of vertice * vector
- (** The origin of the trajectory with the initial direction
- vector. *)
- | Trajectory_vertices of vertice list
- (** The list of vertices for the planned trajectory. *)
+ | Trajectory_vertices of vertice list * (vertice * vertice * vertice * vertice) list
+ (** The list of vertices for the planned trajectory, along with
+ the bezier curves. *)
| Trajectory_set_vertices of vertice list
(** Sets the trajectory. *)
| Trajectory_add_vertice of vertice
diff --git a/info/control2011/src/tools/krobot_planner.ml b/info/control2011/src/tools/krobot_planner.ml
index dec418e..9c4b837 100644
--- a/info/control2011/src/tools/krobot_planner.ml
+++ b/info/control2011/src/tools/krobot_planner.ml
@@ -38,14 +38,12 @@ type planner = {
bus : Krobot_bus.t;
(* The message bus used to communicate with the robot. *)
- mutable origin : (vertice * vector);
- (* 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. *)
-
mutable vertices : vertice list;
(* The list of vertices for the trajectory. *)
+ mutable curves : (vertice * vertice * vertice * vertice) list;
+ (* The parameters of bezier curves. *)
+
mutable moving : bool;
(* Is the robot moving ? *)
@@ -73,13 +71,26 @@ type planner = {
| Primitives |
+-----------------------------------------------------------------+ *)
-let set_vertices planner vertices =
+let set_vertices planner ?curves vertices =
planner.vertices <- vertices;
- ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_vertices vertices))
-
-let set_origin planner (o, v) =
- planner.origin <- (o, v);
- ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_origin(o, v)))
+ (* If the robot is not moving, add its current position to the
+ trajectory. *)
+ let vertices =
+ match planner.moving with
+ | true -> vertices
+ | false -> planner.position :: vertices
+ in
+ (* Compute bezier curves if not provided. *)
+ let curves =
+ match curves with
+ | Some l ->
+ l
+ | None ->
+ let v = { vx = cos planner.orientation; vy = sin planner.orientation } in
+ List.rev (Bezier.fold_vertices (fun p q r s acc -> (p, q, r, s) :: acc) v vertices [])
+ in
+ planner.curves <- curves;
+ ignore (Krobot_bus.send planner.bus (Unix.gettimeofday (), Trajectory_vertices(vertices, curves)))
let set_moving planner moving =
planner.moving <- moving;
@@ -198,11 +209,6 @@ let go planner rotation_speed rotation_acceleration moving_speed moving_accelera
set_moving planner true;
planner.mover <- (
try_lwt
- let o, v = planner.origin in
-
- (* Compute all cubic bezier curves. *)
- let params = List.rev (Bezier.fold_vertices (fun p q r s acc -> (p, q, r, s) :: acc) v (o :: planner.vertices) []) in
-
(* Send a bezier curve to the robot. *)
let send_curve (p, q, r, s) v_end =
(* Compute parameters. *)
@@ -217,55 +223,55 @@ let go planner rotation_speed rotation_acceleration moving_speed moving_accelera
);
(* Send the curve. *)
- Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_bezier(s.x, s.y, d1, d2, theta_end, v_end))
+ lwt () = Krobot_message.send planner.bus (Unix.gettimeofday (), Motor_bezier(s.x, s.y, d1, d2, theta_end, v_end)) in
+
+ return (s, theta_end)
in
(* Remove the first vertice of the trajecotry. *)
- let drop_vertice () =
- (match planner.vertices with
- | _ :: l ->
- set_vertices planner l
- | [] ->
- ());
- set_origin planner (planner.position,
- { vx = cos planner.orientation;
- vy = sin planner.orientation })
+ let drop_vertice (s, theta) =
+ set_vertices planner ~curves:(List.tl planner.curves) (List.tl planner.vertices)
in
- let rec loop = function
+ let rec loop x = function
| [] ->
lwt () = wait_done planner in
- drop_vertice ();
+ set_vertices planner [];
return ()
| [points] ->
lwt () = wait_middle planner in
- lwt () = send_curve points 0.01 in
+ lwt _ = send_curve points 0.01 in
+ lwt () = wait_start planner in
+ drop_vertice x;
lwt () = wait_done planner in
- drop_vertice ();
+ set_vertices planner [];
return ()
| points :: rest ->
lwt () = wait_middle planner in
- lwt () = send_curve points 0.5 in
+ lwt y = send_curve points 0.5 in
lwt () = wait_start planner in
- drop_vertice ();
- loop rest
+ drop_vertice x;
+ loop y rest
in
- match params with
+ (* Add the origin of the trajectory to keep displaying it. *)
+ planner.vertices <- planner.position :: planner.vertices;
+ match planner.curves with
| [] ->
+ set_vertices planner [];
return ()
| [points] ->
- lwt () = send_curve points 0.01 in
+ lwt _ = send_curve points 0.01 in
lwt () = wait_done planner in
- drop_vertice ();
+ set_vertices planner [];
return ()
| points :: rest ->
- lwt () = send_curve points 0.5 in
- loop rest
+ lwt x = send_curve points 0.5 in
+ loop x rest
with exn ->
Lwt_log.error_f ~section ~exn "failed to move"
@@ -287,11 +293,7 @@ let handle_message planner (timestamp, message) =
match decode frame with
| Odometry(x, y, theta) ->
planner.position <- { x; y };
- planner.orientation <- math_mod_float theta (2. *. pi);
- if not planner.moving then
- set_origin planner (planner.position,
- { vx = cos planner.orientation;
- vy = sin planner.orientation })
+ planner.orientation <- math_mod_float theta (2. *. pi)
| Odometry_ghost(x, y, theta, u, following) ->
planner.curve_status <- u;
@@ -308,23 +310,26 @@ let handle_message planner (timestamp, message) =
ignore (
let ts = Unix.gettimeofday () in
join [
- Krobot_bus.send planner.bus (ts, Trajectory_origin(fst planner.origin, snd planner.origin));
- Krobot_bus.send planner.bus (ts, Trajectory_vertices planner.vertices);
+ Krobot_bus.send planner.bus (ts, Trajectory_vertices(planner.vertices, planner.curves));
Krobot_bus.send planner.bus (ts, Trajectory_moving planner.moving);
]
)
| Trajectory_set_vertices l ->
- set_vertices planner l
+ if not planner.moving then
+ set_vertices planner l
| Trajectory_add_vertice vertice ->
- set_vertices planner (planner.vertices @ [vertice])
+ if not planner.moving then
+ set_vertices planner (planner.vertices @ [vertice])
| Trajectory_simplify tolerance ->
- simplify planner tolerance
+ if not planner.moving then
+ simplify planner tolerance
| Trajectory_go(rotation_speed, rotation_acceleration, moving_speed, moving_acceleration) ->
- ignore (go planner rotation_speed rotation_acceleration moving_speed moving_acceleration)
+ if not planner.moving then
+ ignore (go planner rotation_speed rotation_acceleration moving_speed moving_acceleration)
| Trajectory_stop ->
cancel planner.mover;
@@ -371,8 +376,8 @@ lwt () =
(* Create a new planner. *)
let planner = {
bus;
- origin = { x = 0.; y = 0. }, { vx = 1.; vy = 0. };
vertices = [];
+ curves = [];
moving = false;
motors_moving = false;
curve_status = 0;
diff --git a/info/control2011/src/tools/krobot_viewer.ml b/info/control2011/src/tools/krobot_viewer.ml
index 9e22445..7ddee83 100644
--- a/info/control2011/src/tools/krobot_viewer.ml
+++ b/info/control2011/src/tools/krobot_viewer.ml
@@ -50,12 +50,12 @@ type viewer = {
mutable beacon : beacon;
(* The state of the beacon. *)
- mutable origin : vertice * vector;
- (* The origin of the trajectory. *)
-
mutable vertices : vertice list;
(* The current trajectory. *)
+ mutable curves : (vertice * vertice * vertice * vertice) list;
+ (* The current bezier curves of the trajectory. *)
+
mutable motor_status : bool * bool * bool *bool;
(* Status of the four motor controller. *)
}
@@ -268,18 +268,21 @@ let draw viewer =
Cairo.stroke ctx
end;
- let o, v = viewer.origin in
-
(* Draw points. *)
Cairo.set_source_rgb ctx 255. 255. 0.;
- Cairo.move_to ctx o.x o.y;
- List.iter (fun { x; y } -> Cairo.line_to ctx x y) viewer.vertices;
- Cairo.stroke ctx;
+ (match viewer.vertices with
+ | [] ->
+ ()
+ | o :: l ->
+ Cairo.move_to ctx o.x o.y;
+ List.iter (fun { x; y } -> Cairo.line_to ctx x y) l;
+ Cairo.stroke ctx);
(* Draw bezier curves. *)
Cairo.set_source_rgb ctx 255. 0. 255.;
- Bezier.fold_curves
- (fun curve () ->
+ List.iter
+ (fun (p, q, r, s) ->
+ let curve = Bezier.of_vertices p q r s in
let { x; y } = Bezier.vertice curve 0. in
Cairo.move_to ctx x y;
for i = 1 to 100 do
@@ -287,7 +290,7 @@ let draw viewer =
Cairo.line_to ctx x y
done;
Cairo.stroke ctx)
- v (o :: viewer.vertices) ();
+ viewer.curves;
let ctx = Cairo_lablgtk.create viewer.ui#scene#misc#window in
Cairo.set_source_surface ctx surface 0. 0.;
@@ -397,12 +400,9 @@ let handle_message viewer (timestamp, message) =
| Kill "viewer" ->
exit 0
- | Trajectory_origin(o, v) ->
- viewer.origin <- (o, v);
- queue_draw viewer
-
- | Trajectory_vertices l ->
- viewer.vertices <- l;
+ | Trajectory_vertices(vertices, curves) ->
+ viewer.vertices <- vertices;
+ viewer.curves <- curves;
queue_draw viewer
| Trajectory_moving moving ->
@@ -468,8 +468,8 @@ lwt () =
state = init;
ghost = init;
beacon = { xbeacon = 1.; ybeacon = 1.; valid = false };
- origin = ({ x = 0.; y = 0. }, { vx = 0.; vy = 0. });
vertices = [];
+ curves = [];
statusbar_context = ui#statusbar#new_context "";
motor_status = (false, false, false, false);
} in
hooks/post-receive
--
krobot
|
