Skip to content

Commit 37efc70

Browse files
kaaatsu32329kaaatsu32329
committed
Update example for viewer
1 parent bf21889 commit 37efc70

File tree

3 files changed

+237
-67
lines changed

3 files changed

+237
-67
lines changed

‎openrr-nav-viewer/README.md‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
# OpenRR Nav Viewer
2+
3+
## Example
4+
5+
```bash
6+
urdf-viz ./openrr-nav-viewer/urdf/sample.urdf &
7+
cargo run --example urdf_viz_client
8+
```

‎openrr-nav-viewer/examples/urdf_viz_client.rs‎

Lines changed: 157 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,9 @@
11
use arci_urdf_viz::UrdfVizWebClient;
22
use grid_map::*;
3+
use nalgebra as na;
34
use nalgebra::Vector2;
45
use openrr_nav::*;
6+
use openrr_nav_viewer::{BevyAppNav, NavigationViz};
57
use parking_lot::Mutex;
68
use std::sync::Arc;
79

@@ -12,30 +14,64 @@ fn new_sample_map() -> GridMap<u8> {
1214
0.1,
1315
);
1416

15-
let height = map.height();
16-
let width = map.width();
17-
for h in 0..height {
18-
map.set_obstacle(&Grid { x: 0, y: h });
19-
map.set_obstacle(&Grid { x: width, y: h });
17+
for i in 0..40 {
18+
for j in 0..20 {
19+
map.set_obstacle(&Grid {
20+
x: 40 + i,
21+
y: 30 + j,
22+
});
23+
}
2024
}
21-
for w in 0..width {
22-
map.set_obstacle(&Grid { x: w, y: 0 });
23-
map.set_obstacle(&Grid { x: w, y: height });
25+
for i in 0..20 {
26+
for j in 0..30 {
27+
map.set_obstacle(&Grid {
28+
x: 60 + i,
29+
y: 65 + j,
30+
});
31+
}
2432
}
2533

2634
map
2735
}
2836

37+
fn robot_path_from_vec_vec(path: Vec<Vec<f64>>) -> RobotPath {
38+
let mut robot_path_inner = vec![];
39+
for p in path {
40+
let pose = na::Isometry2::new(na::Vector2::new(p[0], p[1]), 0.);
41+
42+
robot_path_inner.push(pose);
43+
}
44+
RobotPath(robot_path_inner)
45+
}
46+
2947
fn main() {
3048
let client = UrdfVizWebClient::default();
3149
client.run_send_velocity_thread();
3250

51+
let nav = NavigationViz::default();
52+
53+
let start = [0.0, 0.0, 0.0];
54+
let goal = [9.0, 8.0, std::f64::consts::FRAC_PI_3];
55+
{
56+
let mut locked_start = nav.start_position.lock();
57+
*locked_start = Pose::new(Vector2::new(start[0], start[1]), start[2]);
58+
let mut locked_goal = nav.goal_position.lock();
59+
*locked_goal = Pose::new(Vector2::new(goal[0], goal[1]), goal[2]);
60+
}
61+
62+
let cloned_nav = nav.clone();
63+
3364
let planner = DwaPlanner::new_from_config(format!(
3465
"{}/../openrr-nav/config/dwa_parameter_config.yaml",
3566
env!("CARGO_MANIFEST_DIR")
3667
))
3768
.unwrap();
3869

70+
{
71+
let mut locked_planner = cloned_nav.planner.lock();
72+
*locked_planner = planner.clone();
73+
}
74+
3975
let mut local_plan_executor = LocalPlanExecutor::new(
4076
Arc::new(Mutex::new(client.clone())),
4177
Arc::new(Mutex::new(client)),
@@ -44,64 +80,118 @@ fn main() {
4480
0.1,
4581
);
4682

47-
let mut map = new_sample_map();
48-
49-
let start = [0.0, 0.0, 0.0];
50-
let goal = [9.0, 8.0, 1.0];
51-
52-
let mut global_plan = GlobalPlan::new(map.clone(), start, goal);
53-
let result = global_plan.global_plan();
54-
55-
let mut cost_maps = CostMaps::new(&map, &result, &start, &goal);
56-
let mut angle_table = AngleTable::new(start[2], goal[2]);
57-
58-
for p in result.iter() {
59-
map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
60-
}
61-
62-
local_plan_executor.set_cost_maps(cost_maps.layered_grid_map());
63-
local_plan_executor.set_angle_table(angle_table.angle_table());
64-
65-
let mut current_pose;
66-
let goal_pose = Pose::new(Vector2::new(goal[0], goal[1]), goal[2]);
67-
68-
const STEP: usize = 1500;
69-
for i in 0..STEP {
70-
current_pose = local_plan_executor.current_pose().unwrap();
71-
72-
cost_maps.update(
73-
&None,
74-
&result,
75-
&[current_pose.translation.x, current_pose.translation.y],
76-
&[],
77-
);
78-
79-
angle_table.update(Some(current_pose), &result);
80-
81-
local_plan_executor.set_cost_maps(cost_maps.layered_grid_map());
82-
local_plan_executor.set_angle_table(angle_table.angle_table());
83-
84-
local_plan_executor.exec_once().unwrap();
85-
86-
println!(
87-
"[ {:4} / {} ] X: {:.3}, Y: {:.3}, THETA: {:.3}",
88-
i + 1,
89-
STEP,
90-
current_pose.translation.x,
91-
current_pose.translation.y,
92-
current_pose.rotation.angle()
93-
);
94-
std::thread::sleep(std::time::Duration::from_millis(5));
95-
96-
const GOAL_THRESHOLD_DISTANCE: f64 = 0.1;
97-
const GOAL_THRESHOLD_ANGLE_DIFFERENCE: f64 = 0.4;
98-
if (goal_pose.translation.vector - current_pose.translation.vector).norm()
99-
< GOAL_THRESHOLD_DISTANCE
100-
&& (goal_pose.rotation.angle() - current_pose.rotation.angle()).abs()
101-
< GOAL_THRESHOLD_ANGLE_DIFFERENCE
102-
{
103-
println!("GOAL! count = {i}");
104-
break;
83+
std::thread::spawn(move || loop {
84+
if cloned_nav.is_run.lock().to_owned() {
85+
let mut map = new_sample_map();
86+
let start;
87+
let goal;
88+
{
89+
let locked_start = cloned_nav.start_position.lock();
90+
start = [
91+
locked_start.translation.x,
92+
locked_start.translation.y,
93+
locked_start.rotation.angle(),
94+
];
95+
let locked_goal = cloned_nav.goal_position.lock();
96+
goal = [
97+
locked_goal.translation.x,
98+
locked_goal.translation.y,
99+
locked_goal.rotation.angle(),
100+
];
101+
}
102+
103+
let mut global_plan = GlobalPlan::new(map.clone(), start, goal);
104+
let result = global_plan.global_plan();
105+
{
106+
let mut locked_robot_path = cloned_nav.robot_path.lock();
107+
locked_robot_path.set_global_path(robot_path_from_vec_vec(result.clone()));
108+
}
109+
110+
let mut cost_maps = CostMaps::new(&map, &result, &start, &goal);
111+
let mut angle_table = AngleTable::new(start[2], goal[2]);
112+
113+
for p in result.iter() {
114+
map.set_value(&map.to_grid(p[0], p[1]).unwrap(), 0).unwrap();
115+
}
116+
117+
local_plan_executor.set_cost_maps(cost_maps.layered_grid_map());
118+
{
119+
let mut locked_layered_grid_map = cloned_nav.layered_grid_map.lock();
120+
*locked_layered_grid_map = cost_maps.layered_grid_map();
121+
}
122+
123+
local_plan_executor.set_angle_table(angle_table.angle_table());
124+
{
125+
let mut locked_angle_table = cloned_nav.angle_table.lock();
126+
*locked_angle_table = angle_table.angle_table();
127+
}
128+
129+
let mut current_pose;
130+
let goal_pose = Pose::new(Vector2::new(goal[0], goal[1]), goal[2]);
131+
132+
const STEP: usize = 2000;
133+
for i in 0..STEP {
134+
current_pose = local_plan_executor.current_pose().unwrap();
135+
136+
cost_maps.update(
137+
&None,
138+
&result,
139+
&[current_pose.translation.x, current_pose.translation.y],
140+
&[],
141+
);
142+
143+
angle_table.update(Some(current_pose), &result);
144+
145+
local_plan_executor.set_cost_maps(cost_maps.layered_grid_map());
146+
{
147+
let mut locked_layered_grid_map = cloned_nav.layered_grid_map.lock();
148+
*locked_layered_grid_map = cost_maps.layered_grid_map();
149+
}
150+
151+
local_plan_executor.set_angle_table(angle_table.angle_table());
152+
{
153+
let mut locked_angle_table = cloned_nav.angle_table.lock();
154+
*locked_angle_table = angle_table.angle_table();
155+
}
156+
157+
local_plan_executor.exec_once().unwrap();
158+
159+
{
160+
let mut locked_robot_pose = cloned_nav.robot_pose.lock();
161+
*locked_robot_pose = current_pose;
162+
}
163+
164+
println!(
165+
"[ {:4} / {} ] X: {:.3}, Y: {:.3}, THETA: {:.3}",
166+
i + 1,
167+
STEP,
168+
current_pose.translation.x,
169+
current_pose.translation.y,
170+
current_pose.rotation.angle()
171+
);
172+
std::thread::sleep(std::time::Duration::from_millis(5));
173+
174+
const GOAL_THRESHOLD_DISTANCE: f64 = 0.1;
175+
const GOAL_THRESHOLD_ANGLE_DIFFERENCE: f64 = 0.4;
176+
if (goal_pose.translation.vector - current_pose.translation.vector).norm()
177+
< GOAL_THRESHOLD_DISTANCE
178+
&& (goal_pose.rotation.angle() - current_pose.rotation.angle()).abs()
179+
< GOAL_THRESHOLD_ANGLE_DIFFERENCE
180+
{
181+
println!("GOAL! count = {i}");
182+
break;
183+
}
184+
}
185+
local_plan_executor.stop().unwrap();
186+
{
187+
let mut is_run = cloned_nav.is_run.lock();
188+
*is_run = false;
189+
}
105190
}
106-
}
191+
});
192+
193+
let bevy_cloned_nav = nav.clone();
194+
let mut app = BevyAppNav::new();
195+
app.setup(bevy_cloned_nav);
196+
app.run();
107197
}

‎openrr-nav-viewer/urdf/sample.urdf‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
<robot name="robot">
2+
<!-- links -->
3+
<link name="plate">
4+
<visual>
5+
<origin xyz="4.5 4.5 -0.1" rpy="0.0 0.0 0.0" />
6+
<geometry>
7+
<box size="11 11 0.2" />
8+
</geometry>
9+
<material name="Gray">
10+
<color rgba="0.5 0.5 0.5 1.0" />
11+
</material>
12+
</visual>
13+
</link>
14+
15+
<link name="obstacle1">
16+
<visual>
17+
<origin xyz="5 3 0.5" rpy="0.0 0.0 0.0" />
18+
<geometry>
19+
<box size="4 2 1" />
20+
</geometry>
21+
<material>
22+
<color rgba="0.8 0.8 1.0 1.0" />
23+
</material>
24+
</visual>
25+
</link>
26+
27+
<link name="obstacle2">
28+
<visual>
29+
<origin xyz="6 7 0.5" rpy="0.0 0.0 0.0" />
30+
<geometry>
31+
<box size="2 3 1" />
32+
</geometry>
33+
<material>
34+
<color rgba="0.8 0.8 1.0 1.0" />
35+
</material>
36+
</visual>
37+
</link>
38+
39+
<link name="goal">
40+
<visual>
41+
<origin xyz="9 8 1" />
42+
<geometry>
43+
<cylinder radius="0.05" length="2" />
44+
</geometry>
45+
<material>
46+
<color rgba="1.0 0.0 0.0 1.0" />
47+
</material>
48+
</visual>
49+
</link>
50+
51+
<link name="root">
52+
<inertial>
53+
<mass value="1" />
54+
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
55+
</inertial>
56+
<visual>
57+
<origin xyz="0 0 0.3" rpy="0 0 0" />
58+
<geometry>
59+
<box size="0.5 0.3 0.6" />
60+
</geometry>
61+
<material name="White">
62+
<color rgba="1 1 1 1" />
63+
</material>
64+
</visual>
65+
<collision>
66+
<origin xyz="0 0 0.3" rpy="0 0 0" />
67+
<geometry>
68+
<box size="0.5 0.3 0.6" />
69+
</geometry>
70+
</collision>
71+
</link>
72+
</robot>

0 commit comments

Comments
 (0)