diff --git a/Cargo.toml b/Cargo.toml
index 31eb362..6b6bd34 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -7,6 +7,10 @@ edition = "2018"
# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
[dependencies]
-nalgebra = "0.21"
-plotlib = "0.5"
-rand = "0.7.3"
+nalgebra = "0.32.4"
+plotlib = "0.5.1"
+rand = "0.8.5"
+rand_distr = "0.4.3"
+itertools = "0.12.1"
+gnuplot = "0.0.42"
+ordered-float = "4.2.0"
diff --git a/README.md b/README.md
index 81528eb..479dd78 100644
--- a/README.md
+++ b/README.md
@@ -20,14 +20,14 @@ cargo run --bin ekf
* [Extended kalman filter localization](#extended-kalman-filter-localization)
* Particle filter localization
* [SLAM](#slam)
- * Iterative Closest Point
+ * Iterative Closest Point
* FastSLAM 1.0
* [Path Planning](#path-planning)
* [Bezier Path](#bezier-path)
* [Cubic Spline](#cubic-spline)
* [Dynamic Window Approach](#dynamic-window-approach)
* [Model Predictive Trajectory Generator](#model-predictive-trajectory-generator)
- * Dijkstra algorithm
+ * [Dijkstra algorithm](#dijkstra-algorithm)
* Potential Field algorithm
* State Lattice Planner
* Rapidly-Exploring Random Trees (RRT)
@@ -41,7 +41,7 @@ cargo run --bin ekf
# Localization
## Extended Kalman Filter Localization
-
+
Red:GPS, Brue:Ground Truth, Green:EKF, Yellow:Dead Reckoning
@@ -62,7 +62,7 @@ cargo run --bin ekf
# Path Planning
## Bezier Path
-
+
Brack:Control points, Green: Path, Red: Start and Goal
@@ -75,7 +75,7 @@ cargo run --bin bezier_path
## Cubic Spline
-
+
Brack:Control points, Green: Path
@@ -88,7 +88,7 @@ cargo run --bin csp
## Dynamic Window Approach
-
+
Brack: Obstacles, Green: Trajectry, Yellow: Predected trajectry
@@ -100,9 +100,9 @@ cargo run --bin dwa
## Model Predictive Trajectory Generator
-
+
-Green: Path
+Green: Path
- [src](https://github.com/rsasaki0109/RustRobotics/blob/master/src/bin/model_predictive_trajectory_generator.rs)
@@ -112,13 +112,18 @@ cargo run --bin model_predictive_trajectory_generator
```
## Dijkstra algorithm
+
+
+
+- [src](./src/bin/dijkstra.rs)
+
## Potential Field algorithm
## State Lattice Planner
## Rapidly-Exploring Random Trees
# Path Tracking
-## Move to Pose
-
+## Move to Pose
+
Green: Path, Red: Start and Goal
@@ -130,9 +135,9 @@ cargo run --bin move_to_pose
## Pure Pursuit
-
+
-Brack: Planned path, Green: Tracked path
+Brack: Planned path, Green: Tracked path
- [src](https://github.com/rsasaki0109/RustRobotics/blob/master/src/bin/pure_pursuit.rs)
@@ -141,25 +146,25 @@ Brack: Planned path, Green: Tracked path
cargo run --bin pure_pursuit
```
-## Stanly Control
+## Stanly Control
-
+
-Brack: Planned path, Green: Tracked path
+Brack: Planned path, Green: Tracked path
- [src](https://github.com/rsasaki0109/RustRobotics/blob/master/src/bin/stanley_controller.rs)
```
-cargo run --bin stanley_control
+cargo run --bin stanley_controller
```
## LQR steer control
-
+
-Brack: Planned path, Green: Tracked path
+Brack: Planned path, Green: Tracked path
- [src](https://github.com/rsasaki0109/RustRobotics/blob/master/src/bin/lqr_steer_control.rs)
diff --git a/img/bezier_path.svg b/img/bezier_path.svg
index 30c0bb2..c26ab93 100644
--- a/img/bezier_path.svg
+++ b/img/bezier_path.svg
@@ -55,14 +55,14 @@ x [m]
-
+
-
+
-4
diff --git a/img/dijkstra_planner.svg b/img/dijkstra_planner.svg
new file mode 100644
index 0000000..4f46c48
--- /dev/null
+++ b/img/dijkstra_planner.svg
@@ -0,0 +1,1360 @@
+
+
+
diff --git a/img/dwa.svg b/img/dwa.svg
index 40fc644..4870d30 100644
--- a/img/dwa.svg
+++ b/img/dwa.svg
@@ -14,7 +14,7 @@
-
+
@@ -58,7 +58,7 @@ x [m]
-
+
@@ -67,7 +67,7 @@ x [m]
-
+
0
diff --git a/img/ekf.svg b/img/ekf.svg
index 8d5c247..a3cf390 100644
--- a/img/ekf.svg
+++ b/img/ekf.svg
@@ -3,506 +3,506 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -1013,1011 +1013,1011 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/img/model_predictive_trajectory_generator.svg b/img/model_predictive_trajectory_generator.svg
index 2163f79..785424b 100644
--- a/img/model_predictive_trajectory_generator.svg
+++ b/img/model_predictive_trajectory_generator.svg
@@ -33,7 +33,7 @@ x [m]
-
+
@@ -41,7 +41,7 @@ x [m]
-
+
-1
diff --git a/img/move_to_pose.svg b/img/move_to_pose.svg
index e9def60..11c317f 100644
--- a/img/move_to_pose.svg
+++ b/img/move_to_pose.svg
@@ -2,12 +2,12 @@
-
+
-
+
@@ -43,14 +43,14 @@ x [m]
-
+
-
+
0
diff --git a/img/stanley_control.svg b/img/stanley_control.svg
index 81c3b75..0332066 100644
--- a/img/stanley_control.svg
+++ b/img/stanley_control.svg
@@ -46,7 +46,7 @@ x [m]
-
+
@@ -54,7 +54,7 @@ x [m]
-
+
-40
diff --git a/media/dijkstra-motion-planner.gif b/media/dijkstra-motion-planner.gif
new file mode 100644
index 0000000..35aed38
Binary files /dev/null and b/media/dijkstra-motion-planner.gif differ
diff --git a/src/bin/dijkstra.rs b/src/bin/dijkstra.rs
new file mode 100644
index 0000000..b69ec92
--- /dev/null
+++ b/src/bin/dijkstra.rs
@@ -0,0 +1,205 @@
+// Dijkstra path planning
+// author: Salah Eddine Ghamri (s.ghamri)
+
+use gnuplot::*;
+use std::thread::sleep;
+use std::time::Duration;
+
+use itertools::iproduct;
+use std::collections::BTreeMap;
+
+use ordered_float::NotNan;
+use std::cmp::Reverse;
+use std::collections::BinaryHeap;
+
+use na::DMatrix;
+extern crate nalgebra as na;
+use rust_robotics::grid_map;
+
+// The neighbors function returns adjacent cell coordinates for a given cell in a grid
+// while excluding out-of-bound and the cell itself.
+fn calculate_neighbors(nrows: usize, ncols: usize) -> impl Fn(usize, usize) -> Vec<(usize, usize)> {
+ move |i, j| {
+ let delta = [-1, 0, 1];
+ let neighbors: Vec<_> = iproduct!(delta, delta)
+ .filter_map(|(di, dj)| {
+ let ni = i as i32 + di;
+ let nj = j as i32 + dj;
+ if ni >= 0
+ && ni < nrows as i32
+ && nj >= 0
+ && nj < ncols as i32
+ && (ni, nj) != (i as i32, j as i32)
+ {
+ Some((ni as usize, nj as usize))
+ } else {
+ None
+ }
+ })
+ .collect();
+ neighbors
+ }
+}
+
+fn has_collision(obstacle_map: &grid_map::Map, ni: usize, nj: usize) -> bool {
+ let (height, width) = obstacle_map.shape();
+ let get_neighbors = calculate_neighbors(height, width);
+
+ let neighbors = get_neighbors(ni, nj);
+
+ if neighbors.iter().all(|&(i, j)| obstacle_map[(i, j)] != 1) && obstacle_map[(ni, nj)] != 1 {
+ return false;
+ }
+
+ true
+}
+
+fn dijkstra_planner(obstacle_map: &grid_map::Map, start: &(usize, usize), goal: &(usize, usize)) {
+ let (height, width) = obstacle_map.shape();
+ let get_neighbors = calculate_neighbors(height, width);
+
+ let mut dist = BTreeMap::new();
+ let mut prev: BTreeMap<(usize, usize), Option<(usize, usize)>> = BTreeMap::new();
+ let mut pq = BinaryHeap::new();
+
+ let (mut ci, mut cj) = start;
+
+ pq.push((Reverse(NotNan::new(0.0).unwrap()), (ci, cj)));
+ dist.insert((ci, cj), 0.0 as f64);
+
+ let mut fg = Figure::new();
+ let mut map_x = vec![];
+ let mut map_y = vec![];
+
+ let mut neighbors_x = vec![];
+ let mut neighbors_y = vec![];
+
+ let mut visited_x = vec![];
+ let mut visited_y = vec![];
+
+ let mut path_x = vec![];
+ let mut path_y = vec![];
+
+ for i in 0..obstacle_map.nrows() {
+ for j in 0..obstacle_map.ncols() {
+ if obstacle_map[(i, j)] == 1 {
+ map_x.push(j as i32);
+ map_y.push(-(i as i32));
+ }
+ }
+ }
+
+ while let Some((distance, current)) = pq.pop() {
+ if current != *goal {
+ (ci, cj) = current;
+ visited_x.push(cj as i32);
+ visited_y.push(-(ci as i32));
+
+ let neighbors = get_neighbors(ci, cj);
+
+ neighbors_x.clear();
+ neighbors_y.clear();
+
+ for (ni, nj) in neighbors {
+ // NOTE: in practice add a collision check with robots radius here
+ if !has_collision(obstacle_map, ni, nj) {
+ neighbors_x.push(nj as i32);
+ neighbors_y.push(-(ni as i32));
+
+ let delta_x = (ci as f64 - ni as f64) as f64;
+ let delta_y = (cj as f64 - nj as f64) as f64;
+ let edge_distance = (delta_x * delta_x + delta_y * delta_y).sqrt();
+ let alt = distance.0.into_inner() + edge_distance;
+
+ if *dist.get(&(ni, nj)).unwrap_or(&f64::MAX) > alt {
+ dist.insert((ni, nj), alt);
+ prev.insert((ni, nj), Some((ci, cj)));
+ pq.push((Reverse(NotNan::new(alt).unwrap()), (ni, nj)));
+ }
+ }
+ }
+ } else {
+ let mut path = vec![current];
+ while let Some(previous) = prev.get(path.last().unwrap()).unwrap_or(&None) {
+ path.push(*previous);
+ if *path.last().unwrap() == *start {
+ path.reverse();
+ path_x.clear();
+ path_y.clear();
+ for point in path {
+ path_x.push(point.1 as i32);
+ path_y.push(-(point.0 as i32));
+ }
+ break;
+ }
+ }
+ }
+
+ fg.clear_axes();
+ fg.axes2d()
+ .set_x_range(Fix(0.0), Fix(width as f64))
+ .set_y_range(Fix(-(height as f64)), Fix(1.0))
+ .points(
+ map_x.iter(),
+ map_y.iter(),
+ &[PointSymbol('S'), Color("black"), PointSize(7.5)],
+ )
+ .points(
+ Some(start.1 as i32),
+ Some(-(start.0 as i32)),
+ &[PointSymbol('O'), Color("red"), PointSize(3.0)],
+ )
+ .points(
+ Some(goal.1),
+ Some(-(goal.0 as i32)),
+ &[PointSymbol('O'), Color("red"), PointSize(3.0)],
+ )
+ .points(
+ &neighbors_x,
+ &neighbors_y,
+ &[PointSymbol('S'), Color("gray"), PointSize(2.0)],
+ )
+ .points(
+ &visited_x,
+ &visited_y,
+ &[PointSymbol('S'), Color("green"), PointSize(2.5)],
+ )
+ .lines(&path_x, &path_y, &[Color("black"), LineWidth(3.0)]);
+
+ let crate_dir = option_env!("CARGO_MANIFEST_DIR").unwrap();
+ fg.show_and_keep_running().unwrap();
+ sleep(Duration::from_millis(10));
+
+ if path_x.len() > 1 {
+ let _ = fg.save_to_svg(
+ format!("{}/img/dijkstra_planner.svg", crate_dir).as_str(),
+ 800,
+ 600,
+ );
+ break;
+ }
+ }
+}
+
+fn main() {
+ #[rustfmt::skip]
+ let original_matrix = DMatrix::from_row_slice(10, 14, &[
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0 ,0 ,0 ,0, 0, 0,
+ 0, 0, 0, 0, 1, 0, 0, 0, 0 ,0 ,0 ,0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1 ,0 ,0 ,0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1 ,0 ,0 ,0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1 ,0 ,0 ,0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1 ,0 ,0 ,0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0
+ ]);
+
+ let obstacle_map = grid_map::Map::new(original_matrix, 3).unwrap();
+
+ let start = (28, 5);
+ let goal = (28, 28);
+
+ dijkstra_planner(&obstacle_map, &start, &goal);
+}
diff --git a/src/bin/ekf.rs b/src/bin/ekf.rs
index f64b54d..eb78d27 100644
--- a/src/bin/ekf.rs
+++ b/src/bin/ekf.rs
@@ -9,7 +9,9 @@ use plotlib::repr::Plot;
use plotlib::view::ContinuousView;
use plotlib::style::{PointMarker, PointStyle};
-use rand::distributions::{Normal, Distribution};
+
+use rand_distr::{Normal, Distribution};
+//use rand::distributions::{Normal, Distribution};
fn motion_model(x: nalgebra::Vector4, u: nalgebra::Vector2, dt: f64)
-> nalgebra::Vector4
@@ -75,11 +77,11 @@ fn ekf_estimation(
let j_h = jacob_h();
let z_pred = observation_model(x_pred);
let y = z - z_pred;
- let s = j_h * p_pred * j_h.transpose() + r;
+ let s = j_h * p_pred * j_h.transpose() + r;
let k = p_pred * j_h.transpose() * s.try_inverse().unwrap();
let new_x_est = x_pred + k * y;
let new_p_est = (nalgebra::Matrix4::identity() - k * j_h) * p_pred;
-
+
(new_x_est , new_p_est)
}
@@ -100,7 +102,7 @@ fn main() {
0., (30.0/180.0 * std::f64::consts::PI).powi(2i32));
let r_sim = nalgebra::Matrix2::::identity();
-
+
let u = nalgebra::Vector2::new(1.0, 0.1);
let mut ud = nalgebra::Vector2::new(0., 0.);
let mut x_dr = nalgebra::Vector4::new(0., 0. , 0., 0.);
@@ -111,13 +113,13 @@ fn main() {
let mut z = nalgebra::Vector2::new(0., 0.);
- let normal = Normal::new(0., 1.); // mean 0., standard deviation 1.
+ let normal = Normal::new(0., 1.).unwrap(); // mean 0., standard deviation 1.
let mut hz = vec![(0., 0.)];
let mut htrue = vec![(0., 0.)];
let mut hdr = vec![(0., 0.)];
let mut hest = vec![(0., 0.)];
-
+
while time < sim_time {
time += dt;
@@ -138,30 +140,30 @@ fn main() {
htrue.push((x_true[0], x_true[1]));
hdr.push((x_dr[0], x_dr[1]));
hest.push((x_est[0], x_est[1]));
-
+
}
-
+
let s0: Plot = Plot::new(hz).point_style(
- PointStyle::new()
+ PointStyle::new()
.colour("#DD3355")
.size(3.),
- );
+ );
let s1: Plot = Plot::new(htrue).point_style(
PointStyle::new()
.colour("#0000ff")
.size(3.),
- );
+ );
let s2: Plot = Plot::new(hdr).point_style(
- PointStyle::new()
+ PointStyle::new()
.colour("#FFFF00")
.size(3.),
- );
+ );
let s3: Plot = Plot::new(hest).point_style(
- PointStyle::new()
+ PointStyle::new()
.colour("#35C788")
.size(3.),
- );
+ );
let v = ContinuousView::new()
.add(s0)
diff --git a/src/grid_map.rs b/src/grid_map.rs
new file mode 100644
index 0000000..7d90fe9
--- /dev/null
+++ b/src/grid_map.rs
@@ -0,0 +1,27 @@
+// grid map definition
+// author:Salah Eddine Ghamri (s.ghamri)
+
+use std::ops::Deref;
+extern crate nalgebra as na;
+
+pub struct Map {
+ grid: na::DMatrix,
+}
+
+impl Map {
+ pub fn new(original_matrix: na::DMatrix, scale: usize) -> Result {
+ if scale < 1 {
+ return Err("scale must be >= 1");
+ }
+ let grid = original_matrix.kronecker(&na::DMatrix::::repeat(scale, scale, 1));
+ Ok(Self { grid })
+ }
+}
+
+impl Deref for Map {
+ type Target = na::DMatrix;
+
+ fn deref(&self) -> &Self::Target {
+ &self.grid
+ }
+}
diff --git a/src/lib.rs b/src/lib.rs
index 97bfb87..557c3f9 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1 +1,2 @@
-pub mod cubic_spline_planner;
\ No newline at end of file
+pub mod cubic_spline_planner;
+pub mod grid_map;