wip

Author: kaaatsu32329

openrr-nav-viewer/Cargo.toml

@@ -7,17 +7,18 @@ edition = "2021"
 
 [dependencies]
 arci = "0.1"
+arci-urdf-viz = "0.1"
 bevy = "0.10"
 bevy_egui = "0.20"
 egui = "0.20"
 nalgebra = "0.30"
 parking_lot = "0.12"
 grid_map = { path = "../grid_map" }
 openrr-nav = { path = "../openrr-nav" }
+yaml-rust = "0.4"
 
 [dev-dependencies]
 arci-ros = "0.1"
-arci-urdf-viz = "0.1"
 tokio = "1"
 rand = "0.8"
 rrt = "0.7"

openrr-nav-viewer/config/example_app.yaml

@@ -0,0 +1,22 @@
+navigation_app_config:
+  init_pose_2d: [0.0, 0.0, 0.0]
+  map: ""
+  localization_client: "UrdfViz"
+  move_base_client: "UrdfViz"
+
+local_plan_executor_config:
+  planner_type: "dwa"
+  frame_id: "frame"
+
+dwa_planner_config:
+  max_velocity: [0.5, 2.0]
+  max_acceleration: [2.0, 5.0]
+  min_velocity: [0.0, -2.0]
+  min_accleration: [-2.0, -5.0]
+  distance_map_weight:
+    path: 0.8
+    goal: 0.9
+    obstacle: 0.3
+  controller_dt: 0.1
+  simulation_duration: 1.0
+  number_of_velocity_sample: 5

openrr-nav-viewer/examples/urdf_viz_client.rs

@@ -30,8 +30,6 @@ fn main() {
     let localization_client = Arc::new(Mutex::new(UrdfVizWebClient::default()));
 
     let mut nav = NavigationVizLite::new(move_base_client, localization_client);
-    nav.start = Arc::new(Mutex::new(Position::new(-0.8, -0.9)));
-    nav.goal = Arc::new(Mutex::new(Position::new(2.5, 0.5)));
     nav.grid_map = Arc::new(Mutex::new(new_square_map()));
 
     let cloned_nav = nav.clone();

openrr-nav-viewer/examples/urdf_viz_client_from_config.rs

@@ -0,0 +1,153 @@
+use arci::Localization;
+use arci_urdf_viz::UrdfVizWebClient;
+use grid_map::*;
+use openrr_nav::*;
+use openrr_nav_viewer::*;
+use parking_lot::Mutex;
+use rand::distributions::{Distribution, Uniform};
+use std::sync::Arc;
+
+fn new_square_map() -> GridMap<u8> {
+    let mut map =
+        grid_map::GridMap::<u8>::new(Position::new(-5.0, -5.0), Position::new(5.0, 5.0), 0.05);
+
+    for i in 0..200 {
+        map.set_obstacle(&Grid { x: 0, y: i });
+        map.set_obstacle(&Grid { x: 199, y: i });
+        map.set_obstacle(&Grid { x: i, y: 0 });
+        map.set_obstacle(&Grid { x: i, y: 199 });
+    }
+
+    map
+}
+
+fn main() {
+    let move_base_client = Arc::new(Mutex::new({
+        let client = UrdfVizWebClient::default();
+        client.run_send_velocity_thread();
+        client
+    }));
+    let localization_client = Arc::new(Mutex::new(UrdfVizWebClient::default()));
+
+    let mut nav = NavigationVizLite::new(move_base_client, localization_client);
+    nav.grid_map = Arc::new(Mutex::new(new_square_map()));
+
+    let cloned_nav = nav.clone();
+
+    std::thread::spawn(move || loop {
+        if cloned_nav.is_run.lock().clone() {
+            let (x_range, y_range) = {
+                let map = cloned_nav.grid_map.lock();
+                (
+                    Uniform::new(map.min_point().x, map.max_point().x),
+                    Uniform::new(map.min_point().y, map.max_point().y),
+                )
+            };
+
+            let goal = {
+                let locked_goal = cloned_nav.goal.lock();
+                [locked_goal.x, locked_goal.y]
+            };
+
+            let start = {
+                let pose = cloned_nav.localization.lock().current_pose("").unwrap();
+                [pose.translation.x, pose.translation.y]
+            };
+
+            let result = rrt::dual_rrt_connect(
+                &start,
+                &goal,
+                |p: &[f64]| {
+                    let map = cloned_nav.grid_map.lock();
+                    !matches!(
+                        map.cell(&map.to_grid(p[0], p[1]).unwrap()).unwrap(),
+                        Cell::Obstacle
+                    )
+                },
+                || {
+                    let mut rng = rand::thread_rng();
+                    vec![x_range.sample(&mut rng), y_range.sample(&mut rng)]
+                },
+                0.05,
+                1000,
+            )
+            .unwrap();
+
+            let path_grid = result
+                .iter()
+                .map(|p| {
+                    let map = cloned_nav.grid_map.lock();
+                    map.to_grid(p[0], p[1]).unwrap()
+                })
+                .collect::<Vec<_>>();
+
+            {
+                let mut grid_map = cloned_nav.grid_map.lock();
+                *grid_map = new_square_map();
+            }
+
+            for p in result {
+                let mut locked_map = cloned_nav.grid_map.lock();
+                let grid = locked_map.to_grid(p[0], p[1]).unwrap();
+                locked_map.set_value(&grid, 0).unwrap();
+            }
+
+            let (path_distance_map, goal_distance_map, obstacle_distance_map) = {
+                let map = cloned_nav.grid_map.lock();
+
+                let goal_grid = map.to_grid(goal[0], goal[1]).unwrap();
+
+                (
+                    path_distance_map(&map, &path_grid).unwrap(),
+                    goal_distance_map(&map, &goal_grid).unwrap(),
+                    obstacle_distance_map(&map).unwrap(),
+                )
+            };
+
+            let mut layered_grid_map = LayeredGridMap::default();
+            layered_grid_map.add_layer(PATH_DISTANCE_MAP_NAME.to_owned(), path_distance_map);
+            layered_grid_map.add_layer(GOAL_DISTANCE_MAP_NAME.to_owned(), goal_distance_map);
+            layered_grid_map
+                .add_layer(OBSTACLE_DISTANCE_MAP_NAME.to_owned(), obstacle_distance_map);
+
+            let mut current_pose;
+            let goal_pose = Pose::new(Vector2::new(goal[0], goal[1]), 0.0);
+
+            let move_base_client = cloned_nav.move_base.clone();
+            let localization_client = cloned_nav.localization.clone();
+
+            let mut executor = local_plan_executor_from_yaml_config(
+                "./config/example_app.yaml",
+                move_base_client,
+                localization_client,
+            )
+            .unwrap();
+            executor.set_cost_maps(layered_grid_map);
+
+            for i in 0..1000 {
+                executor.exec_once().unwrap();
+
+                current_pose = executor.current_pose();
+
+                std::thread::sleep(std::time::Duration::from_millis(50));
+                const GOAL_THRESHOLD: f64 = 0.1;
+                if (goal_pose.translation.vector - current_pose.translation.vector).norm()
+                    < GOAL_THRESHOLD
+                {
+                    println!("GOAL! count = {i}");
+                    break;
+                }
+            }
+            executor.stop();
+            {
+                let mut is_run = cloned_nav.is_run.lock();
+                *is_run = false;
+            }
+        }
+    });
+
+    let bevy_cloned_nav = nav.clone();
+    let mut app = BevyAppLite::new();
+    app.setup(bevy_cloned_nav);
+    app.run();
+}

openrr-nav-viewer/src/bevy_app.rs

@@ -4,12 +4,10 @@ use bevy_egui::{
     EguiContexts, EguiPlugin,
 };
 use grid_map::Position;
+use openrr_nav::{GOAL_DISTANCE_MAP_NAME, OBSTACLE_DISTANCE_MAP_NAME, PATH_DISTANCE_MAP_NAME};
 
 use crate::*;
 
-pub const PATH_DISTANCE_MAP_NAME: &str = "path";
-pub const GOAL_DISTANCE_MAP_NAME: &str = "goal";
-pub const OBSTACLE_DISTANCE_MAP_NAME: &str = "obstacle";
 pub const DEFAULT_PATH_DISTANCE_WEIGHT: f64 = 0.8;
 pub const DEFAULT_GOAL_DISTANCE_WEIGHT: f64 = 0.9;
 pub const DEFAULT_OBSTACLE_DISTANCE_WEIGHT: f64 = 0.3;

openrr-nav-viewer/src/config_reader.rs

@@ -0,0 +1,48 @@
+use arci::{Localization, MoveBase};
+use arci_urdf_viz::UrdfVizWebClient;
+use parking_lot::Mutex;
+use std::{fs, sync::Arc};
+use yaml_rust::YamlLoader;
+
+use crate::{Error, NavigationVizLite};
+
+pub fn navigation_viz_lite_from_yaml_config<M: MoveBase, L: Localization>(
+    path: &str,
+) -> Result<NavigationVizLite<M, L>, Error> {
+    let yaml_file = match fs::read_to_string(path) {
+        Ok(s) => s,
+        Err(_) => return Err(Error::FailedToLoadConfigFile),
+    };
+    let config_vec = YamlLoader::load_from_str(&yaml_file).unwrap();
+    let config = &config_vec[0];
+
+    let move_base;
+    let localization;
+
+    match config["move_base_client"].as_str().unwrap() {
+        "UrdfViz" => {
+            move_base = Arc::new(Mutex::new({
+                let client = UrdfVizWebClient::default();
+                client.run_send_velocity_thread();
+                client
+            }));
+        }
+        _ => {}
+    }
+
+    let mut nav = NavigationVizLite::new(move_base, localization);
+
+    Ok(nav)
+}
+
+#[cfg(test)]
+mod test {
+    use super::*;
+
+    const SAMPLE_YAML_CONFIG_PATH: &str = "./config/example_app.yaml";
+
+    #[test]
+    fn test_navigation_viz_lite_yaml_config() {
+        // let _ = navigation_viz_lite_from_yaml_config(SAMPLE_YAML_CONFIG_PATH);
+    }
+}

openrr-nav-viewer/src/error.rs

@@ -0,0 +1,3 @@
+pub enum Error {
+    FailedToLoadConfigFile,
+}

openrr-nav-viewer/src/lib.rs

@@ -1,11 +1,15 @@
 mod bevy_app;
 mod bevy_app_lite;
+mod config_reader;
 mod converter;
+mod error;
 mod map_type;
 mod nav_viz;
 
 pub use bevy_app::*;
 pub use bevy_app_lite::*;
+pub use config_reader::*;
 pub use converter::*;
+pub use error::*;
 pub use map_type::*;
 pub use nav_viz::*;

openrr-nav/Cargo.toml

@@ -11,6 +11,7 @@ grid_map = "0.1"
 arci = "0.1"
 thiserror = "1"
 parking_lot = "0.12"
+yaml-rust = "0.4"
 
 [dev-dependencies]
 rand = "0.8"

openrr-nav/src/config_reader.rs

@@ -0,0 +1,93 @@
+use arci::{Localization, MoveBase};
+use grid_map::LayeredGridMap;
+use parking_lot::Mutex;
+use std::{collections::HashMap, fs, sync::Arc};
+use yaml_rust::YamlLoader;
+
+use crate::*;
+
+pub fn local_plan_executor_from_yaml_config<M: MoveBase, L: Localization>(
+    path: &str,
+    move_base: Arc<Mutex<M>>,
+    localization: Arc<Mutex<L>>,
+) -> Result<LocalPlanExecutor<M, L>> {
+    let yaml_file = match fs::read_to_string(path) {
+        Ok(s) => s,
+        Err(_) => return Err(Error::FailedToLoadConfigFile("No such file".to_owned())),
+    };
+    let config_vec = YamlLoader::load_from_str(&yaml_file).unwrap();
+    let config = &config_vec[0];
+
+    let local_plane_executor_config = &config["local_plan_executor_config"];
+    let local_planner;
+    match local_plane_executor_config["planner_type"]
+        .as_str()
+        .unwrap()
+    {
+        "dwa" => {
+            let dwa_config = &config["dwa_planner_config"];
+
+            let max_vel = dwa_config["max_velocity"].as_vec().unwrap();
+            let max_accel = dwa_config["max_acceleration"].as_vec().unwrap();
+            let min_vel = dwa_config["min_velocity"].as_vec().unwrap();
+            let min_accel = dwa_config["min_acceleration"].as_vec().unwrap();
+
+            let limits = Limits {
+                max_velocity: Velocity {
+                    x: max_vel[0].as_f64().unwrap(),
+                    theta: max_vel[1].as_f64().unwrap(),
+                },
+                max_accel: Acceleration {
+                    x: max_accel[0].as_f64().unwrap(),
+                    theta: max_accel[1].as_f64().unwrap(),
+                },
+                min_velocity: Velocity {
+                    x: min_vel[0].as_f64().unwrap(),
+                    theta: min_vel[1].as_f64().unwrap(),
+                },
+                min_accel: Acceleration {
+                    x: min_accel[0].as_f64().unwrap(),
+                    theta: min_accel[1].as_f64().unwrap(),
+                },
+            };
+
+            let mut map_name_weight = HashMap::new();
+            map_name_weight.insert(
+                PATH_DISTANCE_MAP_NAME.to_owned(),
+                dwa_config[PATH_DISTANCE_MAP_NAME].as_f64().unwrap(),
+            );
+            map_name_weight.insert(
+                GOAL_DISTANCE_MAP_NAME.to_owned(),
+                dwa_config[GOAL_DISTANCE_MAP_NAME].as_f64().unwrap(),
+            );
+            map_name_weight.insert(
+                OBSTACLE_DISTANCE_MAP_NAME.to_owned(),
+                dwa_config[OBSTACLE_DISTANCE_MAP_NAME].as_f64().unwrap(),
+            );
+
+            local_planner = DwaPlanner::new(
+                limits,
+                map_name_weight,
+                dwa_config["controller_dt"].as_f64().unwrap(),
+                dwa_config["simulation_duration"].as_f64().unwrap(),
+                dwa_config["number_of_velocity_sample"].as_i64().unwrap() as i32,
+            );
+        }
+        _ => {
+            todo!()
+        }
+    }
+
+    let local_plan_executor = LocalPlanExecutor::new(
+        move_base,
+        localization,
+        local_plane_executor_config["frame_id"]
+            .as_str()
+            .unwrap()
+            .to_owned(),
+        local_planner,
+        LayeredGridMap::default(),
+    );
+
+    Ok(local_plan_executor)
+}