|
| 1 | +use arci::{nalgebra as na, Localization}; |
| 2 | +use arci_urdf_viz::UrdfVizWebClient; |
| 3 | +use grid_map::*; |
| 4 | +use openrr_nav::*; |
| 5 | +use openrr_nav_viewer::{BevyAppNav, NavigationViz}; |
| 6 | +use std::sync::{Arc, Mutex}; |
| 7 | + |
| 8 | +fn new_sample_map() -> GridMap<u8> { |
| 9 | + let mut map = grid_map::GridMap::<u8>::new( |
| 10 | + Position::new(-1.05, -1.05), |
| 11 | + Position::new(10.05, 10.05), |
| 12 | + 0.1, |
| 13 | + ); |
| 14 | + |
| 15 | + for i in 0..40 { |
| 16 | + for j in 0..20 { |
| 17 | + map.set_obstacle(&Grid { |
| 18 | + x: 40 + i, |
| 19 | + y: 30 + j, |
| 20 | + }); |
| 21 | + } |
| 22 | + } |
| 23 | + for i in 0..20 { |
| 24 | + for j in 0..30 { |
| 25 | + map.set_obstacle(&Grid { |
| 26 | + x: 60 + i, |
| 27 | + y: 65 + j, |
| 28 | + }); |
| 29 | + } |
| 30 | + } |
| 31 | + |
| 32 | + map |
| 33 | +} |
| 34 | + |
| 35 | +fn robot_path_from_vec_vec(path: Vec<Vec<f64>>) -> RobotPath { |
| 36 | + let mut robot_path_inner = vec![]; |
| 37 | + for p in path { |
| 38 | + let pose = na::Isometry2::new(na::Vector2::new(p[0], p[1]), 0.); |
| 39 | + |
| 40 | + robot_path_inner.push(pose); |
| 41 | + } |
| 42 | + RobotPath(robot_path_inner) |
| 43 | +} |
| 44 | + |
| 45 | +fn main() { |
| 46 | + let client = UrdfVizWebClient::default(); |
| 47 | + client.run_send_velocity_thread(); |
| 48 | + |
| 49 | + let nav = NavigationViz::default(); |
| 50 | + |
| 51 | + let start = client.current_pose("").unwrap(); |
| 52 | + let start = [ |
| 53 | + start.translation.x, |
| 54 | + start.translation.y, |
| 55 | + start.rotation.angle(), |
| 56 | + ]; |
| 57 | + let goal = [9.0, 8.0, std::f64::consts::FRAC_PI_3]; |
| 58 | + { |
| 59 | + let mut locked_start = nav.start_position.lock().unwrap(); |
| 60 | + *locked_start = Pose::new(Vector2::new(start[0], start[1]), start[2]); |
| 61 | + let mut locked_goal = nav.goal_position.lock().unwrap(); |
| 62 | + *locked_goal = Pose::new(Vector2::new(goal[0], goal[1]), goal[2]); |
| 63 | + } |
| 64 | + |
| 65 | + let cloned_nav = nav.clone(); |
| 66 | + |
| 67 | + let planner = DwaPlanner::new_from_config(format!( |
| 68 | + "{}/../openrr-nav/config/dwa_parameter_config.yaml", |
| 69 | + env!("CARGO_MANIFEST_DIR") |
| 70 | + )) |
| 71 | + .unwrap(); |
| 72 | + |
| 73 | + { |
| 74 | + let mut locked_planner = cloned_nav.planner.lock().unwrap(); |
| 75 | + *locked_planner = planner.clone(); |
| 76 | + } |
| 77 | + |
| 78 | + let mut local_plan_executor = LocalPlanExecutor::new( |
| 79 | + Arc::new(Mutex::new(client.clone())), |
| 80 | + Arc::new(Mutex::new(client.clone())), |
| 81 | + "".to_owned(), |
| 82 | + planner, |
| 83 | + 0.1, |
| 84 | + ); |
| 85 | + |
| 86 | + std::thread::spawn(move || loop { |
| 87 | + if cloned_nav.is_run.lock().unwrap().clone() { |
| 88 | + let mut map = new_sample_map(); |
| 89 | + let start; |
| 90 | + let goal; |
| 91 | + { |
| 92 | + let current_pose = client.current_pose("").unwrap(); |
| 93 | + let mut locked_start = cloned_nav.start_position.lock().unwrap(); |
| 94 | + *locked_start = current_pose; |
| 95 | + start = [ |
| 96 | + current_pose.translation.x, |
| 97 | + current_pose.translation.y, |
| 98 | + current_pose.rotation.angle(), |
| 99 | + ]; |
| 100 | + |
| 101 | + let locked_goal = cloned_nav.goal_position.lock().unwrap(); |
| 102 | + goal = [ |
| 103 | + locked_goal.translation.x, |
| 104 | + locked_goal.translation.y, |
| 105 | + locked_goal.rotation.angle(), |
| 106 | + ]; |
| 107 | + } |
| 108 | + |
| 109 | + let mut global_plan = GlobalPlan::new(map.clone(), start, goal); |
| 110 | + |
| 111 | + let result = global_plan.global_plan(); |
| 112 | + { |
| 113 | + let mut locked_robot_path = cloned_nav.robot_path.lock().unwrap(); |
| 114 | + locked_robot_path.set_global_path(robot_path_from_vec_vec(result.clone())); |
| 115 | + } |
| 116 | + |
| 117 | + let mut cost_maps = CostMaps::new(&map, &result, &start, &goal); |
| 118 | + let mut angle_table = AngleTable::new(start[2], goal[2]); |
| 119 | + |
| 120 | + for p in result.iter() { |
| 121 | + map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap(); |
| 122 | + } |
| 123 | + |
| 124 | + local_plan_executor.set_cost_maps(cost_maps.layered_grid_map()); |
| 125 | + { |
| 126 | + let mut locked_layered_grid_map = cloned_nav.layered_grid_map.lock().unwrap(); |
| 127 | + *locked_layered_grid_map = cost_maps.layered_grid_map(); |
| 128 | + } |
| 129 | + |
| 130 | + local_plan_executor.set_angle_table(angle_table.angle_table()); |
| 131 | + { |
| 132 | + let mut locked_angle_table = cloned_nav.angle_table.lock().unwrap(); |
| 133 | + *locked_angle_table = angle_table.angle_table(); |
| 134 | + } |
| 135 | + |
| 136 | + let mut current_pose; |
| 137 | + let goal_pose = Pose::new(Vector2::new(goal[0], goal[1]), goal[2]); |
| 138 | + |
| 139 | + const STEP: usize = 2000; |
| 140 | + for i in 0..STEP { |
| 141 | + current_pose = local_plan_executor.current_pose().unwrap(); |
| 142 | + |
| 143 | + cost_maps.update( |
| 144 | + &None, |
| 145 | + &result, |
| 146 | + &[current_pose.translation.x, current_pose.translation.y], |
| 147 | + &[], |
| 148 | + ); |
| 149 | + |
| 150 | + angle_table.update(Some(current_pose), &result); |
| 151 | + |
| 152 | + local_plan_executor.set_cost_maps(cost_maps.layered_grid_map()); |
| 153 | + { |
| 154 | + let mut locked_layered_grid_map = cloned_nav.layered_grid_map.lock().unwrap(); |
| 155 | + *locked_layered_grid_map = cost_maps.layered_grid_map(); |
| 156 | + } |
| 157 | + |
| 158 | + local_plan_executor.set_angle_table(angle_table.angle_table()); |
| 159 | + { |
| 160 | + let mut locked_angle_table = cloned_nav.angle_table.lock().unwrap(); |
| 161 | + *locked_angle_table = angle_table.angle_table(); |
| 162 | + } |
| 163 | + |
| 164 | + local_plan_executor.exec_once().unwrap(); |
| 165 | + |
| 166 | + { |
| 167 | + let mut locked_robot_pose = cloned_nav.robot_pose.lock().unwrap(); |
| 168 | + *locked_robot_pose = current_pose; |
| 169 | + } |
| 170 | + |
| 171 | + println!( |
| 172 | + "[ {:4} / {} ] X: {:.3}, Y: {:.3}, THETA: {:.3}", |
| 173 | + i + 1, |
| 174 | + STEP, |
| 175 | + current_pose.translation.x, |
| 176 | + current_pose.translation.y, |
| 177 | + current_pose.rotation.angle() |
| 178 | + ); |
| 179 | + std::thread::sleep(std::time::Duration::from_millis(5)); |
| 180 | + |
| 181 | + const GOAL_THRESHOLD_DISTANCE: f64 = 0.1; |
| 182 | + const GOAL_THRESHOLD_ANGLE_DIFFERENCE: f64 = 0.4; |
| 183 | + if (goal_pose.translation.vector - current_pose.translation.vector).norm() |
| 184 | + < GOAL_THRESHOLD_DISTANCE |
| 185 | + && (goal_pose.rotation.angle() - current_pose.rotation.angle()).abs() |
| 186 | + < GOAL_THRESHOLD_ANGLE_DIFFERENCE |
| 187 | + { |
| 188 | + println!("GOAL! count = {i}"); |
| 189 | + break; |
| 190 | + } |
| 191 | + } |
| 192 | + local_plan_executor.stop().unwrap(); |
| 193 | + { |
| 194 | + let mut is_run = cloned_nav.is_run.lock().unwrap(); |
| 195 | + *is_run = false; |
| 196 | + } |
| 197 | + } |
| 198 | + }); |
| 199 | + |
| 200 | + let bevy_cloned_nav = nav.clone(); |
| 201 | + let mut app = BevyAppNav::new(); |
| 202 | + app.setup(bevy_cloned_nav); |
| 203 | + app.run(); |
| 204 | +} |
0 commit comments