1use nalgebra::{Vector2, Vector4, Vector5};
8
9use rust_robotics_core::{
10 ControlInput, Obstacles, Path2D, Point2D, RoboticsError, RoboticsResult, State2D,
11};
12
13const ROBOT_STUCK_FLAG_CONS: f64 = 0.001;
14
15fn snap_to_resolution(value: f64, resolution: f64) -> f64 {
16 if !value.is_finite() || !resolution.is_finite() || resolution <= 0.0 {
17 return value;
18 }
19 (value / resolution).round() * resolution
20}
21
22fn arange_samples(start: f64, stop: f64, step: f64) -> Vec<f64> {
23 if !(start.is_finite() && stop.is_finite() && step.is_finite()) || step <= 0.0 || start >= stop
24 {
25 return Vec::new();
26 }
27 let count = ((stop - start) / step).ceil().max(0.0) as usize;
28 (0..count)
29 .map(|index| snap_to_resolution(start + step * index as f64, step))
30 .collect()
31}
32
33pub type DWAState = Vector5<f64>;
35
36pub type DWAControl = Vector2<f64>;
38
39#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
40pub enum DWARobotType {
41 #[default]
42 Circle,
43 Rectangle,
44}
45
46fn rectangle_collision(
47 robot_x: f64,
48 robot_y: f64,
49 robot_yaw: f64,
50 obstacle_x: f64,
51 obstacle_y: f64,
52 robot_width: f64,
53 robot_length: f64,
54) -> bool {
55 let relative_x = obstacle_x - robot_x;
56 let relative_y = obstacle_y - robot_y;
57 let local_x = relative_x * robot_yaw.cos() + relative_y * robot_yaw.sin();
58 let local_y = -relative_x * robot_yaw.sin() + relative_y * robot_yaw.cos();
59 local_x <= robot_length / 2.0
60 && local_x >= -robot_length / 2.0
61 && local_y <= robot_width / 2.0
62 && local_y >= -robot_width / 2.0
63}
64
65#[derive(Debug, Clone)]
67pub struct DWAConfig {
68 pub max_speed: f64,
69 pub min_speed: f64,
70 pub max_yaw_rate: f64,
71 pub max_accel: f64,
72 pub max_delta_yaw_rate: f64,
73 pub v_resolution: f64,
74 pub yaw_rate_resolution: f64,
75 pub dt: f64,
76 pub predict_time: f64,
77 pub to_goal_cost_gain: f64,
78 pub speed_cost_gain: f64,
79 pub obstacle_cost_gain: f64,
80 pub robot_type: DWARobotType,
81 pub robot_radius: f64,
82 pub robot_width: f64,
83 pub robot_length: f64,
84 pub goal_threshold: f64,
85}
86
87impl Default for DWAConfig {
88 fn default() -> Self {
89 Self {
90 max_speed: 1.0,
91 min_speed: -0.5,
92 max_yaw_rate: 40.0_f64.to_radians(),
93 max_accel: 0.2,
94 max_delta_yaw_rate: 40.0_f64.to_radians(),
95 v_resolution: 0.01,
96 yaw_rate_resolution: 0.1_f64.to_radians(),
97 dt: 0.1,
98 predict_time: 3.0,
99 to_goal_cost_gain: 0.15,
100 speed_cost_gain: 1.0,
101 obstacle_cost_gain: 1.0,
102 robot_type: DWARobotType::Circle,
103 robot_radius: 1.0,
104 robot_width: 0.5,
105 robot_length: 1.2,
106 goal_threshold: 1.0,
107 }
108 }
109}
110
111impl DWAConfig {
112 pub fn validate(&self) -> RoboticsResult<()> {
113 if !self.max_speed.is_finite()
114 || !self.min_speed.is_finite()
115 || self.min_speed > self.max_speed
116 {
117 return Err(RoboticsError::InvalidParameter(
118 "DWA speed range must be finite and min_speed must be <= max_speed".to_string(),
119 ));
120 }
121 if !self.max_yaw_rate.is_finite() || self.max_yaw_rate <= 0.0 {
122 return Err(RoboticsError::InvalidParameter(
123 "DWA max_yaw_rate must be positive and finite".to_string(),
124 ));
125 }
126 if !self.max_accel.is_finite() || self.max_accel <= 0.0 {
127 return Err(RoboticsError::InvalidParameter(
128 "DWA max_accel must be positive and finite".to_string(),
129 ));
130 }
131 if !self.max_delta_yaw_rate.is_finite() || self.max_delta_yaw_rate <= 0.0 {
132 return Err(RoboticsError::InvalidParameter(
133 "DWA max_delta_yaw_rate must be positive and finite".to_string(),
134 ));
135 }
136 if !self.v_resolution.is_finite() || self.v_resolution <= 0.0 {
137 return Err(RoboticsError::InvalidParameter(
138 "DWA v_resolution must be positive and finite".to_string(),
139 ));
140 }
141 if !self.yaw_rate_resolution.is_finite() || self.yaw_rate_resolution <= 0.0 {
142 return Err(RoboticsError::InvalidParameter(
143 "DWA yaw_rate_resolution must be positive and finite".to_string(),
144 ));
145 }
146 if !self.dt.is_finite() || self.dt <= 0.0 {
147 return Err(RoboticsError::InvalidParameter(
148 "DWA dt must be positive and finite".to_string(),
149 ));
150 }
151 if !self.predict_time.is_finite() || self.predict_time <= 0.0 {
152 return Err(RoboticsError::InvalidParameter(
153 "DWA predict_time must be positive and finite".to_string(),
154 ));
155 }
156 if !self.to_goal_cost_gain.is_finite() || self.to_goal_cost_gain < 0.0 {
157 return Err(RoboticsError::InvalidParameter(
158 "DWA to_goal_cost_gain must be non-negative and finite".to_string(),
159 ));
160 }
161 if !self.speed_cost_gain.is_finite() || self.speed_cost_gain < 0.0 {
162 return Err(RoboticsError::InvalidParameter(
163 "DWA speed_cost_gain must be non-negative and finite".to_string(),
164 ));
165 }
166 if !self.obstacle_cost_gain.is_finite() || self.obstacle_cost_gain < 0.0 {
167 return Err(RoboticsError::InvalidParameter(
168 "DWA obstacle_cost_gain must be non-negative and finite".to_string(),
169 ));
170 }
171 if !self.robot_width.is_finite() || self.robot_width < 0.0 {
172 return Err(RoboticsError::InvalidParameter(
173 "DWA robot_width must be non-negative and finite".to_string(),
174 ));
175 }
176 if !self.robot_length.is_finite() || self.robot_length < 0.0 {
177 return Err(RoboticsError::InvalidParameter(
178 "DWA robot_length must be non-negative and finite".to_string(),
179 ));
180 }
181 if self.robot_type == DWARobotType::Rectangle
182 && (self.robot_width <= 0.0 || self.robot_length <= 0.0)
183 {
184 return Err(RoboticsError::InvalidParameter(
185 "DWA rectangle robot dimensions must be positive".to_string(),
186 ));
187 }
188 if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
189 return Err(RoboticsError::InvalidParameter(
190 "DWA robot_radius must be non-negative and finite".to_string(),
191 ));
192 }
193 if !self.goal_threshold.is_finite() || self.goal_threshold < 0.0 {
194 return Err(RoboticsError::InvalidParameter(
195 "DWA goal_threshold must be non-negative and finite".to_string(),
196 ));
197 }
198 Ok(())
199 }
200}
201
202pub type DynamicWindow = Vector4<f64>;
204
205#[derive(Debug, Clone)]
207pub struct DWACandidate {
208 pub trajectory: Trajectory,
209 pub goal_cost: f64,
211 pub speed_cost: f64,
212 pub obstacle_cost: Option<f64>,
214}
215
216#[derive(Debug, Clone)]
222pub struct DWAPlanTrace {
223 pub dynamic_window: DynamicWindow,
224 pub candidates: Vec<DWACandidate>,
225 pub selected: Option<usize>,
226 pub applied_control: DWAControl,
227 pub stuck_override: bool,
228}
229
230#[derive(Debug, Clone)]
232pub struct Trajectory {
233 pub states: Vec<DWAState>,
234 pub control: DWAControl,
235 pub cost: f64,
236}
237
238impl Trajectory {
239 pub fn new() -> Self {
240 Self {
241 states: Vec::new(),
242 control: DWAControl::zeros(),
243 cost: f64::MAX,
244 }
245 }
246
247 pub fn final_state(&self) -> Option<&DWAState> {
248 self.states.last()
249 }
250
251 pub fn to_path(&self) -> Path2D {
252 let points: Vec<Point2D> = self
253 .states
254 .iter()
255 .map(|s| Point2D::new(s[0], s[1]))
256 .collect();
257 Path2D::from_points(points)
258 }
259}
260
261impl Default for Trajectory {
262 fn default() -> Self {
263 Self::new()
264 }
265}
266
267pub struct DWAPlanner {
269 config: DWAConfig,
270 state: DWAState,
271 goal: Point2D,
272 obstacles: Vec<Point2D>,
273 best_trajectory: Trajectory,
274}
275
276impl DWAPlanner {
277 pub fn new(config: DWAConfig) -> Self {
278 Self::try_new(config).expect("invalid DWA configuration")
279 }
280
281 pub fn try_new(config: DWAConfig) -> RoboticsResult<Self> {
282 config.validate()?;
283 Ok(Self {
284 config,
285 state: DWAState::zeros(),
286 goal: Point2D::origin(),
287 obstacles: Vec::new(),
288 best_trajectory: Trajectory::new(),
289 })
290 }
291
292 pub fn with_defaults() -> Self {
293 Self::new(DWAConfig::default())
294 }
295
296 pub fn set_state(&mut self, state: DWAState) {
297 self.try_set_state(state)
298 .expect("DWA state must contain only finite values")
299 }
300
301 pub fn try_set_state(&mut self, state: DWAState) -> RoboticsResult<()> {
302 Self::validate_state(&state)?;
303 self.state = state;
304 Ok(())
305 }
306
307 pub fn set_state_from_2d(&mut self, state: &State2D, omega: f64) {
308 self.try_set_state_from_2d(state, omega)
309 .expect("DWA state must contain only finite values")
310 }
311
312 pub fn try_set_state_from_2d(&mut self, state: &State2D, omega: f64) -> RoboticsResult<()> {
313 self.try_set_state(DWAState::new(state.x, state.y, state.yaw, state.v, omega))
314 }
315
316 pub fn get_state(&self) -> &DWAState {
317 &self.state
318 }
319
320 pub fn state_2d(&self) -> State2D {
321 State2D::new(self.state[0], self.state[1], self.state[2], self.state[3])
322 }
323
324 pub fn set_goal(&mut self, goal: Point2D) {
325 self.try_set_goal(goal)
326 .expect("DWA goal must contain only finite values")
327 }
328
329 pub fn try_set_goal(&mut self, goal: Point2D) -> RoboticsResult<()> {
330 Self::validate_goal(&goal)?;
331 self.goal = goal;
332 Ok(())
333 }
334
335 pub fn set_obstacles(&mut self, obstacles: Vec<Point2D>) {
336 self.try_set_obstacles(obstacles)
337 .expect("DWA obstacles must contain only finite values")
338 }
339
340 pub fn try_set_obstacles(&mut self, obstacles: Vec<Point2D>) -> RoboticsResult<()> {
341 Self::validate_obstacles(&obstacles)?;
342 self.obstacles = obstacles;
343 Ok(())
344 }
345
346 pub fn set_obstacles_from_tuples(&mut self, obstacles: &[(f64, f64)]) {
347 self.try_set_obstacles(
348 obstacles
349 .iter()
350 .map(|(x, y)| Point2D::new(*x, *y))
351 .collect(),
352 )
353 .expect("DWA obstacles must contain only finite values")
354 }
355
356 pub fn set_obstacles_from_obstacles(&mut self, obstacles: &Obstacles) -> RoboticsResult<()> {
357 self.try_set_obstacles(obstacles.points.clone())
358 }
359
360 pub fn get_best_trajectory(&self) -> &Trajectory {
361 &self.best_trajectory
362 }
363 pub fn best_path(&self) -> Path2D {
364 self.best_trajectory.to_path()
365 }
366 pub fn config(&self) -> &DWAConfig {
367 &self.config
368 }
369
370 fn motion(&self, state: &DWAState, control: &DWAControl) -> DWAState {
371 let dt = self.config.dt;
372 let mut next = *state;
373 next[2] += control[1] * dt;
374 next[0] += control[0] * next[2].cos() * dt;
375 next[1] += control[0] * next[2].sin() * dt;
376 next[3] = control[0];
377 next[4] = control[1];
378 next
379 }
380
381 fn calc_dynamic_window(&self) -> DynamicWindow {
382 let config = &self.config;
383 let state = &self.state;
384 let vs = Vector4::new(
385 config.min_speed,
386 config.max_speed,
387 -config.max_yaw_rate,
388 config.max_yaw_rate,
389 );
390 let vd = Vector4::new(
391 state[3] - config.max_accel * config.dt,
392 state[3] + config.max_accel * config.dt,
393 state[4] - config.max_delta_yaw_rate * config.dt,
394 state[4] + config.max_delta_yaw_rate * config.dt,
395 );
396 DynamicWindow::new(
397 vs[0].max(vd[0]),
398 vs[1].min(vd[1]),
399 vs[2].max(vd[2]),
400 vs[3].min(vd[3]),
401 )
402 }
403
404 fn predict_trajectory(&self, v: f64, omega: f64) -> Trajectory {
405 let config = &self.config;
406 let mut state = self.state;
407 let control = DWAControl::new(v, omega);
408 let mut states = Vec::new();
409 states.push(state);
410 let mut time = 0.0;
411 while time <= config.predict_time {
412 state = self.motion(&state, &control);
413 states.push(state);
414 time += config.dt;
415 }
416 Trajectory {
417 states,
418 control,
419 cost: 0.0,
420 }
421 }
422
423 fn calc_to_goal_cost(&self, trajectory: &Trajectory) -> f64 {
424 if let Some(final_state) = trajectory.final_state() {
425 let dx = self.goal.x - final_state[0];
426 let dy = self.goal.y - final_state[1];
427 let target_angle = dy.atan2(dx);
428 let heading_error = target_angle - final_state[2];
429 heading_error.sin().atan2(heading_error.cos()).abs()
430 } else {
431 f64::MAX
432 }
433 }
434
435 fn calc_speed_cost(&self, trajectory: &Trajectory) -> f64 {
436 if let Some(final_state) = trajectory.final_state() {
437 self.config.max_speed - final_state[3]
438 } else {
439 f64::MAX
440 }
441 }
442
443 fn calc_obstacle_cost(&self, trajectory: &Trajectory) -> f64 {
444 if self.obstacles.is_empty() {
445 return 0.0;
446 }
447 let mut min_dist = f64::MAX;
448 for state in &trajectory.states {
449 for obs in &self.obstacles {
450 let dx = state[0] - obs.x;
451 let dy = state[1] - obs.y;
452 let dist = (dx * dx + dy * dy).sqrt();
453 let collision = match self.config.robot_type {
454 DWARobotType::Circle => dist <= self.config.robot_radius,
455 DWARobotType::Rectangle => rectangle_collision(
456 state[0],
457 state[1],
458 state[2],
459 obs.x,
460 obs.y,
461 self.config.robot_width,
462 self.config.robot_length,
463 ),
464 };
465 if collision {
466 return f64::MAX;
467 }
468 if dist < min_dist {
469 min_dist = dist;
470 }
471 }
472 }
473 if min_dist < f64::MAX {
474 1.0 / min_dist
475 } else {
476 0.0
477 }
478 }
479
480 pub fn plan_step(&mut self) -> DWAControl {
481 self.try_plan_step().expect("invalid DWA planning state")
482 }
483
484 pub fn try_plan_step(&mut self) -> RoboticsResult<DWAControl> {
485 self.plan_step_internal(None)
486 }
487
488 pub fn try_plan_step_with_trace(&mut self) -> RoboticsResult<DWAPlanTrace> {
491 let mut trace = DWAPlanTrace {
492 dynamic_window: self.calc_dynamic_window(),
493 candidates: Vec::new(),
494 selected: None,
495 applied_control: DWAControl::zeros(),
496 stuck_override: false,
497 };
498 self.plan_step_internal(Some(&mut trace))?;
499 Ok(trace)
500 }
501
502 fn plan_step_internal(
503 &mut self,
504 mut trace: Option<&mut DWAPlanTrace>,
505 ) -> RoboticsResult<DWAControl> {
506 self.config.validate()?;
507 Self::validate_state(&self.state)?;
508 Self::validate_goal(&self.goal)?;
509 Self::validate_obstacles(&self.obstacles)?;
510
511 let config = &self.config;
512 let dw = self.calc_dynamic_window();
513 let mut best_trajectory = Trajectory {
514 states: vec![self.state],
515 control: DWAControl::zeros(),
516 cost: f64::MAX,
517 };
518 let mut min_cost = f64::MAX;
519
520 for v in arange_samples(dw[0], dw[1], config.v_resolution) {
521 for omega in arange_samples(dw[2], dw[3], config.yaw_rate_resolution) {
522 let mut trajectory = self.predict_trajectory(v, omega);
523 let raw_obstacle_cost = self.calc_obstacle_cost(&trajectory);
529 if raw_obstacle_cost == f64::MAX {
530 if let Some(t) = trace.as_deref_mut() {
531 let goal_cost =
532 config.to_goal_cost_gain * self.calc_to_goal_cost(&trajectory);
533 let speed_cost = config.speed_cost_gain * self.calc_speed_cost(&trajectory);
534 trajectory.cost = f64::MAX;
535 t.candidates.push(DWACandidate {
536 trajectory,
537 goal_cost,
538 speed_cost,
539 obstacle_cost: None,
540 });
541 }
542 continue;
543 }
544 let goal_cost = config.to_goal_cost_gain * self.calc_to_goal_cost(&trajectory);
545 let speed_cost = config.speed_cost_gain * self.calc_speed_cost(&trajectory);
546 let obstacle_cost = config.obstacle_cost_gain * raw_obstacle_cost;
547 let total_cost = goal_cost + speed_cost + obstacle_cost;
548 if let Some(t) = trace.as_deref_mut() {
549 trajectory.cost = total_cost;
550 t.candidates.push(DWACandidate {
551 trajectory: trajectory.clone(),
552 goal_cost,
553 speed_cost,
554 obstacle_cost: Some(obstacle_cost),
555 });
556 }
557 if total_cost <= min_cost {
558 min_cost = total_cost;
559 let mut control = DWAControl::new(v, omega);
560 if control[0].abs() < ROBOT_STUCK_FLAG_CONS
561 && self.state[3].abs() < ROBOT_STUCK_FLAG_CONS
562 {
563 control[1] = -config.max_delta_yaw_rate;
564 }
565 if let Some(t) = trace.as_deref_mut() {
566 t.selected = Some(t.candidates.len() - 1);
567 t.stuck_override = control[1] != omega;
568 }
569 best_trajectory = Trajectory {
570 states: trajectory.states,
571 control,
572 cost: total_cost,
573 };
574 }
575 }
576 }
577
578 self.best_trajectory = best_trajectory;
579 if let Some(t) = trace {
580 t.applied_control = self.best_trajectory.control;
581 }
582 Ok(self.best_trajectory.control)
583 }
584
585 pub fn step(&mut self) -> DWAControl {
586 self.try_step().expect("invalid DWA step")
587 }
588
589 pub fn try_step(&mut self) -> RoboticsResult<DWAControl> {
590 let control = self.try_plan_step()?;
591 self.state = self.motion(&self.state, &control);
592 Ok(control)
593 }
594
595 pub fn is_goal_reached(&self) -> bool {
596 let dx = self.state[0] - self.goal.x;
597 let dy = self.state[1] - self.goal.y;
598 (dx * dx + dy * dy).sqrt() <= self.config.goal_threshold
599 }
600
601 pub fn distance_to_goal(&self) -> f64 {
602 let dx = self.state[0] - self.goal.x;
603 let dy = self.state[1] - self.goal.y;
604 (dx * dx + dy * dy).sqrt()
605 }
606
607 pub fn navigate_to_goal(&mut self, max_steps: usize) -> Vec<DWAState> {
608 self.try_navigate_to_goal(max_steps)
609 .expect("invalid DWA navigation")
610 }
611
612 pub fn try_navigate_to_goal(&mut self, max_steps: usize) -> RoboticsResult<Vec<DWAState>> {
613 let mut path = vec![self.state];
614 for _ in 0..max_steps {
615 if self.is_goal_reached() {
616 break;
617 }
618 self.try_step()?;
619 path.push(self.state);
620 }
621 Ok(path)
622 }
623
624 pub fn try_plan_input(&mut self) -> RoboticsResult<ControlInput> {
625 let control = self.try_plan_step()?;
626 Ok(Self::control_to_input(&control))
627 }
628
629 pub fn control_to_input(control: &DWAControl) -> ControlInput {
630 ControlInput::new(control[0], control[1])
631 }
632
633 fn validate_state(state: &DWAState) -> RoboticsResult<()> {
634 if state.iter().any(|value| !value.is_finite()) {
635 return Err(RoboticsError::InvalidParameter(
636 "DWA state must contain only finite values".to_string(),
637 ));
638 }
639 Ok(())
640 }
641
642 fn validate_goal(goal: &Point2D) -> RoboticsResult<()> {
643 if !goal.x.is_finite() || !goal.y.is_finite() {
644 return Err(RoboticsError::InvalidParameter(
645 "DWA goal must contain only finite values".to_string(),
646 ));
647 }
648 Ok(())
649 }
650
651 fn validate_obstacles(obstacles: &[Point2D]) -> RoboticsResult<()> {
652 if obstacles
653 .iter()
654 .any(|o| !o.x.is_finite() || !o.y.is_finite())
655 {
656 return Err(RoboticsError::InvalidParameter(
657 "DWA obstacles must contain only finite values".to_string(),
658 ));
659 }
660 Ok(())
661 }
662}
663
664#[derive(Debug, Clone)]
666#[deprecated(note = "Use DWAConfig instead")]
667pub struct Config {
668 pub max_speed: f64,
669 pub min_speed: f64,
670 pub max_yaw_rate: f64,
671 pub max_accel: f64,
672 pub max_delta_yaw_rate: f64,
673 pub v_resolution: f64,
674 pub yaw_rate_resolution: f64,
675 pub dt: f64,
676 pub predict_time: f64,
677 pub to_goal_cost_gain: f64,
678 pub speed_cost_gain: f64,
679 pub obstacle_cost_gain: f64,
680 pub robot_type: DWARobotType,
681 pub robot_radius: f64,
682 pub robot_width: f64,
683 pub robot_length: f64,
684}
685
686pub fn motion(mut x: Vector5<f64>, u: Vector2<f64>, dt: f64) -> Vector5<f64> {
687 x[2] += u[1] * dt;
688 x[0] += u[0] * x[2].cos() * dt;
689 x[1] += u[0] * x[2].sin() * dt;
690 x[3] = u[0];
691 x[4] = u[1];
692 x
693}
694
695#[allow(deprecated)]
696pub fn calc_dynamic_window(x: Vector5<f64>, config: &Config) -> Vector4<f64> {
697 let vs = Vector4::new(
698 config.min_speed,
699 config.max_speed,
700 -config.max_yaw_rate,
701 config.max_yaw_rate,
702 );
703 let vd = Vector4::new(
704 x[3] - config.max_accel * config.dt,
705 x[3] + config.max_accel * config.dt,
706 x[4] - config.max_delta_yaw_rate * config.dt,
707 x[4] + config.max_delta_yaw_rate * config.dt,
708 );
709 Vector4::new(
710 vs[0].max(vd[0]),
711 vs[1].min(vd[1]),
712 vs[2].max(vd[2]),
713 vs[3].min(vd[3]),
714 )
715}
716
717#[allow(deprecated)]
718pub fn predict_trajectory(
719 x_init: Vector5<f64>,
720 v: f64,
721 y: f64,
722 config: &Config,
723) -> Vec<Vector5<f64>> {
724 let mut x = x_init;
725 let mut trajectory = vec![x_init];
726 let mut time = 0.0;
727 while time <= config.predict_time {
728 x = motion(x, Vector2::new(v, y), config.dt);
729 trajectory.push(x);
730 time += config.dt;
731 }
732 trajectory
733}
734
735#[allow(deprecated)]
736pub fn calc_obstacle_cost(trajectory: &[Vector5<f64>], ob: &[(f64, f64)], config: &Config) -> f64 {
737 if trajectory.is_empty() || ob.is_empty() {
738 return 0.0;
739 }
740 let mut min_r = f64::MAX;
741 for state in trajectory {
742 for obstacle in ob {
743 let r = ((state[0] - obstacle.0).powi(2) + (state[1] - obstacle.1).powi(2)).sqrt();
744 let collision = match config.robot_type {
745 DWARobotType::Circle => r <= config.robot_radius,
746 DWARobotType::Rectangle => rectangle_collision(
747 state[0],
748 state[1],
749 state[2],
750 obstacle.0,
751 obstacle.1,
752 config.robot_width,
753 config.robot_length,
754 ),
755 };
756 if collision {
757 return f64::MAX;
758 }
759 if r < min_r {
760 min_r = r;
761 }
762 }
763 }
764 if min_r < f64::MAX {
765 1.0 / min_r
766 } else {
767 0.0
768 }
769}
770
771pub fn calc_to_goal_cost(trajectory: &[Vector5<f64>], goal: (f64, f64)) -> f64 {
772 if trajectory.is_empty() {
773 return f64::MAX;
774 }
775 let last = &trajectory[trajectory.len() - 1];
776 let dx = goal.0 - last[0];
777 let dy = goal.1 - last[1];
778 let error_angle = dy.atan2(dx);
779 let cost_angle = error_angle - last[2];
780 cost_angle.sin().atan2(cost_angle.cos()).abs()
781}
782
783#[allow(deprecated)]
784pub fn dwa_control(
785 x: Vector5<f64>,
786 config: &Config,
787 goal: (f64, f64),
788 ob: &[(f64, f64)],
789) -> ((f64, f64), Vec<Vector5<f64>>) {
790 let dw = calc_dynamic_window(x, config);
791 calc_control_and_trajectory(x, dw, config, goal, ob)
792}
793
794#[allow(deprecated)]
795pub fn calc_control_and_trajectory(
796 x: Vector5<f64>,
797 dw: Vector4<f64>,
798 config: &Config,
799 goal: (f64, f64),
800 ob: &[(f64, f64)],
801) -> ((f64, f64), Vec<Vector5<f64>>) {
802 let mut min_cost = f64::MAX;
803 let mut best_u = (0.0, 0.0);
804 let mut best_trajectory = vec![x];
805 for v in arange_samples(dw[0], dw[1], config.v_resolution) {
806 for y in arange_samples(dw[2], dw[3], config.yaw_rate_resolution) {
807 let trajectory = predict_trajectory(x, v, y, config);
808 let to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(&trajectory, goal);
809 let speed_cost =
810 config.speed_cost_gain * (config.max_speed - trajectory.last().unwrap()[3]);
811 let ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(&trajectory, ob, config);
812 let final_cost = to_goal_cost + speed_cost + ob_cost;
813 if final_cost <= min_cost {
814 min_cost = final_cost;
815 best_u = (v, y);
816 best_trajectory = trajectory;
817 if best_u.0.abs() < ROBOT_STUCK_FLAG_CONS && x[3].abs() < ROBOT_STUCK_FLAG_CONS {
818 best_u.1 = -config.max_delta_yaw_rate;
819 }
820 }
821 }
822 }
823 (best_u, best_trajectory)
824}
825
826#[cfg(test)]
827mod tests {
828 use super::*;
829
830 #[derive(Clone, Copy)]
831 struct ClosedLoopSample {
832 control: [f64; 2],
833 state: [f64; 5],
834 predicted_len: usize,
835 predicted_last: [f64; 5],
836 dist: f64,
837 }
838
839 fn assert_close(actual: f64, expected: f64) {
840 assert!(
841 (actual - expected).abs() < 1.0e-12,
842 "expected {expected}, got {actual}"
843 );
844 }
845
846 fn assert_close_tol(actual: f64, expected: f64, tol: f64) {
847 assert!(
848 (actual - expected).abs() < tol,
849 "expected {expected}, got {actual} (tol={tol})"
850 );
851 }
852
853 fn assert_state_close(actual: &DWAState, expected: &[f64; 5]) {
854 for (value, expected) in actual.iter().zip(expected.iter()) {
855 assert_close(*value, *expected);
856 }
857 }
858
859 fn assert_state_close_tol(actual: &[f64; 5], expected: &[f64; 5], tol: f64) {
860 for (value, expected) in actual.iter().zip(expected.iter()) {
861 assert_close_tol(*value, *expected, tol);
862 }
863 }
864
865 fn assert_sample_close(actual: &ClosedLoopSample, expected: &ClosedLoopSample) {
866 assert_close_tol(actual.control[0], expected.control[0], 0.05);
867 assert_close_tol(actual.control[1], expected.control[1], 0.05);
868 assert_state_close_tol(&actual.state, &expected.state, 0.25);
869 assert_eq!(actual.predicted_len, expected.predicted_len);
870 assert_state_close_tol(&actual.predicted_last, &expected.predicted_last, 0.25);
871 assert_close_tol(actual.dist, expected.dist, 0.2);
872 }
873
874 fn to_state_array(state: &DWAState) -> [f64; 5] {
875 [state[0], state[1], state[2], state[3], state[4]]
876 }
877
878 fn run_closed_loop_trace(
879 mut dwa: DWAPlanner,
880 max_steps: usize,
881 ) -> RoboticsResult<Vec<ClosedLoopSample>> {
882 let mut trace = Vec::new();
883 for _step in 0..max_steps {
884 let control = dwa.try_plan_step()?;
885 let predicted = dwa.get_best_trajectory();
886 let predicted_len = predicted.states.len();
887 let predicted_last = to_state_array(predicted.final_state().unwrap());
888 let next_state = dwa.motion(&dwa.state, &control);
889 dwa.state = next_state;
890 trace.push(ClosedLoopSample {
891 control: [control[0], control[1]],
892 state: to_state_array(&dwa.state),
893 predicted_len,
894 predicted_last,
895 dist: dwa.distance_to_goal(),
896 });
897 if dwa.is_goal_reached() {
898 break;
899 }
900 }
901 Ok(trace)
902 }
903
904 fn pythonrobotics_default_obstacles() -> Vec<Point2D> {
905 vec![
906 Point2D::new(-1.0, -1.0),
907 Point2D::new(0.0, 2.0),
908 Point2D::new(4.0, 2.0),
909 Point2D::new(5.0, 4.0),
910 Point2D::new(5.0, 5.0),
911 Point2D::new(5.0, 6.0),
912 Point2D::new(5.0, 9.0),
913 Point2D::new(8.0, 9.0),
914 Point2D::new(7.0, 9.0),
915 Point2D::new(8.0, 10.0),
916 Point2D::new(9.0, 11.0),
917 Point2D::new(12.0, 13.0),
918 Point2D::new(12.0, 12.0),
919 Point2D::new(15.0, 15.0),
920 Point2D::new(13.0, 13.0),
921 ]
922 }
923
924 fn pythonrobotics_stuck_obstacles() -> Vec<Point2D> {
925 vec![
926 Point2D::new(1.0, 1.0),
927 Point2D::new(-0.0, -2.0),
928 Point2D::new(-2.0, -6.0),
929 Point2D::new(-2.0, -8.0),
930 Point2D::new(-3.0, -9.27),
931 Point2D::new(-3.79, -9.39),
932 Point2D::new(-7.25, -8.97),
933 Point2D::new(-7.0, -2.0),
934 Point2D::new(-3.0, -4.0),
935 Point2D::new(-6.0, -5.0),
936 Point2D::new(-3.5, -5.8),
937 Point2D::new(-6.0, -9.0),
938 Point2D::new(-8.8, -9.0),
939 Point2D::new(-5.0, -9.0),
940 Point2D::new(-7.5, -3.0),
941 Point2D::new(-9.0, -8.0),
942 Point2D::new(-5.8, -4.4),
943 Point2D::new(-12.0, -12.0),
944 Point2D::new(-3.0, -2.0),
945 Point2D::new(-13.0, -13.0),
946 ]
947 }
948
949 #[test]
950 fn trace_preserves_decisions_including_ties_rejections_and_stuck_override() {
951 for config in [
952 DWAConfig::default(),
953 DWAConfig {
954 obstacle_cost_gain: 0.0,
955 ..DWAConfig::default()
956 },
957 ] {
958 for blocked in [false, true] {
959 for speed in [0.0, 0.6] {
960 let mut a = DWAPlanner::new(config.clone());
961 let mut b = DWAPlanner::new(config.clone());
962 let state = DWAState::new(0., 0., 0., speed, 0.);
963 for p in [&mut a, &mut b] {
964 p.set_state(state);
965 p.set_goal(Point2D::new(5., 0.));
966 if blocked {
967 p.set_obstacles(vec![Point2D::origin()]);
968 }
969 }
970 let expected = a.plan_step();
971 let trace = b.try_plan_step_with_trace().unwrap();
972 assert_eq!(expected, trace.applied_control);
973 assert_eq!(
974 a.get_best_trajectory().states,
975 b.get_best_trajectory().states
976 );
977 assert_eq!(a.get_best_trajectory().cost, b.get_best_trajectory().cost);
978 assert!(!trace.candidates.is_empty());
979 if blocked {
980 assert!(trace.selected.is_none());
981 assert!(trace.candidates.iter().all(|c| c.obstacle_cost.is_none()));
982 assert_eq!(expected, DWAControl::zeros());
983 } else {
984 let i = trace.selected.unwrap();
985 let cost = trace.candidates[i].trajectory.cost;
986 assert!(trace.candidates.iter().all(|c| c.trajectory.cost >= cost));
987 assert_eq!(
988 trace
989 .candidates
990 .iter()
991 .rposition(|c| c.trajectory.cost == cost),
992 Some(i)
993 );
994 }
995 }
996 }
997 }
998 let mut p = DWAPlanner::new(DWAConfig {
999 min_speed: 0.,
1000 max_speed: 0.01,
1001 v_resolution: 1.,
1002 ..DWAConfig::default()
1003 });
1004 p.set_goal(Point2D::new(5., 0.));
1005 let t = p.try_plan_step_with_trace().unwrap();
1006 assert!(t.stuck_override);
1007 assert_eq!(t.applied_control[1], -p.config().max_delta_yaw_rate);
1008 assert_ne!(
1009 t.applied_control,
1010 t.candidates[t.selected.unwrap()].trajectory.control
1011 );
1012 let mut p = DWAPlanner::new(DWAConfig {
1015 min_speed: 0.,
1016 max_speed: 0.01,
1017 v_resolution: 1.,
1018 max_yaw_rate: 1.2,
1019 max_delta_yaw_rate: 2.,
1020 dt: 0.2,
1021 ..DWAConfig::default()
1022 });
1023 p.set_goal(Point2D::new(5., 0.));
1024 let first = p.try_plan_step_with_trace().unwrap();
1025 assert!(first.stuck_override);
1026 p.set_state(motion(*p.get_state(), first.applied_control, 0.2));
1027 let next = p.try_plan_step_with_trace().unwrap();
1028 assert!(next.dynamic_window[2] > next.dynamic_window[3]);
1029 assert!(next.candidates.is_empty());
1030 assert!(next.selected.is_none());
1031 assert_eq!(next.applied_control, DWAControl::zeros());
1032 assert_eq!(p.plan_step(), next.applied_control);
1033 }
1034
1035 #[test]
1036 fn test_dwa_creation() {
1037 let dwa = DWAPlanner::with_defaults();
1038 assert_eq!(dwa.state, DWAState::zeros());
1039 }
1040
1041 #[test]
1042 fn test_dwa_config_default() {
1043 let config = DWAConfig::default();
1044 assert_eq!(config.max_speed, 1.0);
1045 assert_eq!(config.robot_type, DWARobotType::Circle);
1046 assert_eq!(config.robot_radius, 1.0);
1047 assert_eq!(config.robot_width, 0.5);
1048 assert_eq!(config.robot_length, 1.2);
1049 }
1050
1051 #[test]
1052 fn test_dwa_set_goal() {
1053 let mut dwa = DWAPlanner::with_defaults();
1054 dwa.set_goal(Point2D::new(10.0, 10.0));
1055 assert_eq!(dwa.goal.x, 10.0);
1056 assert_eq!(dwa.goal.y, 10.0);
1057 }
1058
1059 #[test]
1060 fn test_dwa_motion() {
1061 let dwa = DWAPlanner::with_defaults();
1062 let state = DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0);
1063 let control = DWAControl::new(1.0, 0.0);
1064 let next = dwa.motion(&state, &control);
1065 assert!(next[0] > 0.0);
1066 assert_eq!(next[3], 1.0);
1067 }
1068
1069 #[test]
1070 fn test_dwa_dynamic_window() {
1071 let mut dwa = DWAPlanner::with_defaults();
1072 dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.5, 0.0));
1073 let dw = dwa.calc_dynamic_window();
1074 assert!(dw[0] >= dwa.config.min_speed);
1075 assert!(dw[1] <= dwa.config.max_speed);
1076 }
1077
1078 #[test]
1079 fn test_dwa_predict_trajectory() {
1080 let dwa = DWAPlanner::with_defaults();
1081 let trajectory = dwa.predict_trajectory(1.0, 0.0);
1082 assert!(!trajectory.states.is_empty());
1083 let final_state = trajectory.final_state().unwrap();
1084 assert!(final_state[0] > 0.0);
1085 }
1086
1087 #[test]
1088 fn test_dwa_obstacle_cost() {
1089 let mut dwa = DWAPlanner::with_defaults();
1090 dwa.set_obstacles(vec![Point2D::new(100.0, 100.0)]);
1091 let trajectory = dwa.predict_trajectory(1.0, 0.0);
1092 let cost = dwa.calc_obstacle_cost(&trajectory);
1093 assert!(cost < 1.0);
1094 }
1095
1096 #[test]
1097 fn test_dwa_collision_detection() {
1098 let mut dwa = DWAPlanner::with_defaults();
1099 dwa.set_obstacles(vec![Point2D::new(0.5, 0.0)]);
1100 let trajectory = dwa.predict_trajectory(1.0, 0.0);
1101 let cost = dwa.calc_obstacle_cost(&trajectory);
1102 assert_eq!(cost, f64::MAX);
1103 }
1104
1105 #[test]
1106 fn test_dwa_plan_step() {
1107 let mut dwa = DWAPlanner::with_defaults();
1108 dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1109 dwa.set_goal(Point2D::new(10.0, 0.0));
1110 let control = dwa.plan_step();
1111 assert!(control[0] > 0.0);
1112 }
1113
1114 #[test]
1115 fn test_dwa_goal_reached() {
1116 let mut dwa = DWAPlanner::with_defaults();
1117 dwa.set_state(DWAState::new(10.0, 10.0, 0.0, 0.0, 0.0));
1118 dwa.set_goal(Point2D::new(10.0, 10.0));
1119 assert!(dwa.is_goal_reached());
1120 }
1121
1122 #[test]
1123 fn test_dwa_navigate() {
1124 let mut dwa = DWAPlanner::with_defaults();
1125 dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1126 dwa.set_goal(Point2D::new(3.0, 0.0));
1127 let path = dwa.navigate_to_goal(100);
1128 assert!(path.len() > 1);
1129 let final_state = path.last().unwrap();
1130 let dist = ((final_state[0] - 3.0).powi(2) + final_state[1].powi(2)).sqrt();
1131 assert!(dist <= dwa.config.goal_threshold);
1132 }
1133
1134 #[test]
1135 fn test_trajectory_to_path() {
1136 let dwa = DWAPlanner::with_defaults();
1137 let trajectory = dwa.predict_trajectory(1.0, 0.1);
1138 let path = trajectory.to_path();
1139 assert!(!path.is_empty());
1140 }
1141
1142 #[test]
1143 fn test_legacy_motion() {
1144 let x = Vector5::new(0.0, 0.0, 0.0, 0.0, 0.0);
1145 let u = Vector2::new(1.0, 0.0);
1146 let next = motion(x, u, 0.1);
1147 assert!(next[0] > 0.0);
1148 }
1149
1150 #[test]
1151 fn test_dwa_try_new_rejects_invalid_config() {
1152 let config = DWAConfig {
1153 dt: 0.0,
1154 ..Default::default()
1155 };
1156 let err = match DWAPlanner::try_new(config) {
1157 Ok(_) => panic!("expected invalid config to be rejected"),
1158 Err(err) => err,
1159 };
1160 assert!(matches!(err, RoboticsError::InvalidParameter(_)));
1161 }
1162
1163 #[test]
1164 fn test_dwa_state_2d_matches_internal_state() {
1165 let mut dwa = DWAPlanner::with_defaults();
1166 dwa.set_state(DWAState::new(1.0, 2.0, 0.3, 0.4, 0.1));
1167 let state = dwa.state_2d();
1168 assert_eq!(state.x, 1.0);
1169 assert_eq!(state.y, 2.0);
1170 assert_eq!(state.yaw, 0.3);
1171 assert_eq!(state.v, 0.4);
1172 }
1173
1174 #[test]
1175 fn test_dwa_set_obstacles_from_obstacles() {
1176 let mut dwa = DWAPlanner::with_defaults();
1177 let obstacles =
1178 Obstacles::from_points(vec![Point2D::new(1.0, 1.0), Point2D::new(2.0, 2.0)]);
1179 dwa.set_obstacles_from_obstacles(&obstacles).unwrap();
1180 assert_eq!(dwa.obstacles.len(), 2);
1181 }
1182
1183 #[test]
1184 fn test_dwa_try_plan_input_returns_common_control() {
1185 let mut dwa = DWAPlanner::with_defaults();
1186 dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1187 dwa.set_goal(Point2D::new(10.0, 0.0));
1188 let control = dwa.try_plan_input().unwrap();
1189 assert!(control.v > 0.0);
1190 }
1191
1192 #[test]
1193 fn test_dwa_best_path_after_planning() {
1194 let mut dwa = DWAPlanner::with_defaults();
1195 dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.0, 0.0));
1196 dwa.set_goal(Point2D::new(3.0, 0.0));
1197 dwa.try_plan_step().unwrap();
1198 let best_path = dwa.best_path();
1199 assert!(!best_path.is_empty());
1200 }
1201
1202 #[test]
1203 fn test_legacy_obstacle_cost_checks_all_states_and_obstacles() {
1204 #[allow(deprecated)]
1205 let config = Config {
1206 max_speed: 1.0,
1207 min_speed: -0.5,
1208 max_yaw_rate: 40.0_f64.to_radians(),
1209 max_accel: 0.2,
1210 max_delta_yaw_rate: 40.0_f64.to_radians(),
1211 v_resolution: 0.01,
1212 yaw_rate_resolution: 0.1_f64.to_radians(),
1213 dt: 0.1,
1214 predict_time: 3.0,
1215 to_goal_cost_gain: 0.15,
1216 speed_cost_gain: 1.0,
1217 obstacle_cost_gain: 1.0,
1218 robot_type: DWARobotType::Circle,
1219 robot_radius: 0.5,
1220 robot_width: 0.5,
1221 robot_length: 1.2,
1222 };
1223 let trajectory = vec![
1224 Vector5::new(0.0, 0.0, 0.0, 0.0, 0.0),
1225 Vector5::new(2.0, 2.0, 0.0, 0.0, 0.0),
1226 ];
1227 let obstacles = [(10.0, 10.0), (2.0, 2.0)];
1228 #[allow(deprecated)]
1229 let cost = calc_obstacle_cost(&trajectory, &obstacles, &config);
1230 assert_eq!(cost, f64::MAX);
1231 }
1232
1233 #[test]
1234 fn an_all_colliding_window_stops_instead_of_picking_an_arc() {
1235 for gain in [0.3, 1.0, 2.0] {
1236 let mut dwa = DWAPlanner::new(DWAConfig {
1237 obstacle_cost_gain: gain,
1238 ..DWAConfig::default()
1239 });
1240 dwa.set_state(DWAState::new(0.0, 0.0, 0.0, 0.5, 0.0));
1241 dwa.set_goal(Point2D::new(10.0, 0.0));
1242 dwa.set_obstacles(
1244 (0..36)
1245 .map(|k| {
1246 let a = k as f64 * std::f64::consts::TAU / 36.0;
1247 Point2D::new(1.2 * a.cos(), 1.2 * a.sin())
1248 })
1249 .collect(),
1250 );
1251 let control = dwa.try_plan_step().expect("plan");
1252 assert_eq!(dwa.get_best_trajectory().cost, f64::MAX, "gain {gain}");
1253 assert_eq!(control[0], 0.0, "gain {gain}: drove at {}", control[0]);
1254 }
1255 }
1256
1257 #[test]
1258 fn test_dwa_rectangle_collision_detection_respects_local_frame() {
1259 let mut dwa = DWAPlanner::new(DWAConfig {
1260 robot_type: DWARobotType::Rectangle,
1261 robot_radius: 0.2,
1262 robot_width: 0.5,
1263 robot_length: 1.2,
1264 ..DWAConfig::default()
1265 });
1266 dwa.set_state(DWAState::new(
1267 0.0,
1268 0.0,
1269 std::f64::consts::FRAC_PI_2,
1270 0.0,
1271 0.0,
1272 ));
1273 dwa.set_obstacles(vec![Point2D::new(0.0, 0.55)]);
1274
1275 let trajectory = Trajectory {
1276 states: vec![*dwa.get_state()],
1277 control: DWAControl::zeros(),
1278 cost: 0.0,
1279 };
1280 let cost = dwa.calc_obstacle_cost(&trajectory);
1281 assert_eq!(cost, f64::MAX);
1282
1283 #[allow(deprecated)]
1284 let legacy_config = Config {
1285 max_speed: 1.0,
1286 min_speed: -0.5,
1287 max_yaw_rate: 40.0_f64.to_radians(),
1288 max_accel: 0.2,
1289 max_delta_yaw_rate: 40.0_f64.to_radians(),
1290 v_resolution: 0.01,
1291 yaw_rate_resolution: 0.1_f64.to_radians(),
1292 dt: 0.1,
1293 predict_time: 3.0,
1294 to_goal_cost_gain: 0.15,
1295 speed_cost_gain: 1.0,
1296 obstacle_cost_gain: 1.0,
1297 robot_type: DWARobotType::Rectangle,
1298 robot_radius: 0.2,
1299 robot_width: 0.5,
1300 robot_length: 1.2,
1301 };
1302 #[allow(deprecated)]
1303 let legacy_cost = calc_obstacle_cost(&trajectory.states, &[(0.0, 0.55)], &legacy_config);
1304 assert_eq!(legacy_cost, f64::MAX);
1305 }
1306
1307 #[test]
1308 fn test_dwa_default_main_initial_parity_matches_pythonrobotics() {
1309 let mut dwa = DWAPlanner::with_defaults();
1310 dwa.set_state(DWAState::new(
1311 0.0,
1312 0.0,
1313 std::f64::consts::FRAC_PI_8,
1314 0.0,
1315 0.0,
1316 ));
1317 dwa.set_goal(Point2D::new(10.0, 10.0));
1318 dwa.set_obstacles(pythonrobotics_default_obstacles());
1319
1320 let control = dwa.try_plan_step().unwrap();
1321 assert_close(control[0], 0.02);
1322 assert_close(control[1], 0.068_067_840_827_779_04);
1323
1324 let trajectory = &dwa.get_best_trajectory().states;
1325 assert_eq!(trajectory.len(), 31);
1326 assert_state_close(
1327 &trajectory[0],
1328 &[0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0],
1329 );
1330 assert_state_close(
1331 &trajectory[1],
1332 &[
1333 0.001_842_506_612_952_407,
1334 0.000_777_926_334_061_682_8,
1335 0.399_505_865_781_502_05,
1336 0.02,
1337 0.068_067_840_827_779_04,
1338 ],
1339 );
1340 assert_state_close(
1341 &trajectory[2],
1342 &[
1343 0.003_679_675_406_577_515,
1344 0.001_568_376_094_470_316,
1345 0.406_312_649_864_279_95,
1346 0.02,
1347 0.068_067_840_827_779_04,
1348 ],
1349 );
1350 assert_state_close(
1351 &trajectory[5],
1352 &[
1353 0.009_158_304_932_256_946,
1354 0.004_014_496_810_584_791,
1355 0.426_733_002_112_613_67,
1356 0.02,
1357 0.068_067_840_827_779_04,
1358 ],
1359 );
1360 assert_state_close(
1361 &trajectory[10],
1362 &[
1363 0.018_174_703_618_670_7,
1364 0.008_338_301_686_158_353,
1365 0.460_766_922_526_503_2,
1366 0.02,
1367 0.068_067_840_827_779_04,
1368 ],
1369 );
1370 assert_state_close(
1371 &trajectory[20],
1372 &[
1373 0.035_740_187_678_676_45,
1374 0.017_893_451_940_005_592,
1375 0.528_834_763_354_282_3,
1376 0.02,
1377 0.068_067_840_827_779_04,
1378 ],
1379 );
1380 assert_state_close(
1381 &trajectory[30],
1382 &[
1383 0.052_615_098_653_616_07,
1384 0.028_621_196_634_198_105,
1385 0.596_902_604_182_061_3,
1386 0.02,
1387 0.068_067_840_827_779_04,
1388 ],
1389 );
1390
1391 #[allow(deprecated)]
1392 let legacy_config = Config {
1393 max_speed: 1.0,
1394 min_speed: -0.5,
1395 max_yaw_rate: 40.0_f64.to_radians(),
1396 max_accel: 0.2,
1397 max_delta_yaw_rate: 40.0_f64.to_radians(),
1398 v_resolution: 0.01,
1399 yaw_rate_resolution: 0.1_f64.to_radians(),
1400 dt: 0.1,
1401 predict_time: 3.0,
1402 to_goal_cost_gain: 0.15,
1403 speed_cost_gain: 1.0,
1404 obstacle_cost_gain: 1.0,
1405 robot_type: DWARobotType::Circle,
1406 robot_radius: 1.0,
1407 robot_width: 0.5,
1408 robot_length: 1.2,
1409 };
1410 let obstacle_tuples: Vec<_> = pythonrobotics_default_obstacles()
1411 .into_iter()
1412 .map(|point| (point.x, point.y))
1413 .collect();
1414 #[allow(deprecated)]
1415 let (legacy_u, legacy_trajectory) = dwa_control(
1416 DWAState::new(0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0),
1417 &legacy_config,
1418 (10.0, 10.0),
1419 &obstacle_tuples,
1420 );
1421 assert_close(legacy_u.0, control[0]);
1422 assert_close(legacy_u.1, control[1]);
1423 assert_eq!(legacy_trajectory.len(), trajectory.len());
1424 assert_state_close(
1425 legacy_trajectory.last().unwrap(),
1426 &[
1427 0.052_615_098_653_616_07,
1428 0.028_621_196_634_198_105,
1429 0.596_902_604_182_061_3,
1430 0.02,
1431 0.068_067_840_827_779_04,
1432 ],
1433 );
1434 }
1435
1436 #[test]
1437 fn test_dwa_stuck_initial_control_matches_pythonrobotics() {
1438 let mut dwa = DWAPlanner::new(DWAConfig {
1439 to_goal_cost_gain: 0.2,
1440 obstacle_cost_gain: 2.0,
1441 ..DWAConfig::default()
1442 });
1443 dwa.set_state(DWAState::new(
1444 0.0,
1445 0.0,
1446 std::f64::consts::FRAC_PI_8,
1447 0.0,
1448 0.0,
1449 ));
1450 dwa.set_goal(Point2D::new(-5.0, -7.0));
1451 dwa.set_obstacles(pythonrobotics_stuck_obstacles());
1452
1453 let control = dwa.try_plan_step().unwrap();
1454 assert!(control[0].abs() < 1.0e-15);
1455 assert_close(control[1], -0.698_131_700_797_731_8);
1456
1457 let trajectory = &dwa.get_best_trajectory().states;
1458 assert_eq!(trajectory.len(), 31);
1459 assert_state_close(
1460 &trajectory[1],
1461 &[
1462 -3.214_541_934_276_051_4e-19,
1463 -1.305_290_122_999_866_4e-19,
1464 0.385_717_764_690_746_8,
1465 -3.469_446_951_953_614e-18,
1466 -0.069_813_170_079_773_18,
1467 ],
1468 );
1469 assert_state_close(
1470 &trajectory[30],
1471 &[
1472 -9.971_752_160_697_17e-18,
1473 -2.915_944_394_677_270_7e-18,
1474 0.183_259_571_459_404_3,
1475 -3.469_446_951_953_614e-18,
1476 -0.069_813_170_079_773_18,
1477 ],
1478 );
1479
1480 #[allow(deprecated)]
1481 let legacy_config = Config {
1482 max_speed: 1.0,
1483 min_speed: -0.5,
1484 max_yaw_rate: 40.0_f64.to_radians(),
1485 max_accel: 0.2,
1486 max_delta_yaw_rate: 40.0_f64.to_radians(),
1487 v_resolution: 0.01,
1488 yaw_rate_resolution: 0.1_f64.to_radians(),
1489 dt: 0.1,
1490 predict_time: 3.0,
1491 to_goal_cost_gain: 0.2,
1492 speed_cost_gain: 1.0,
1493 obstacle_cost_gain: 2.0,
1494 robot_type: DWARobotType::Circle,
1495 robot_radius: 1.0,
1496 robot_width: 0.5,
1497 robot_length: 1.2,
1498 };
1499 let obstacle_tuples: Vec<_> = pythonrobotics_stuck_obstacles()
1500 .into_iter()
1501 .map(|point| (point.x, point.y))
1502 .collect();
1503 #[allow(deprecated)]
1504 let (legacy_u, legacy_trajectory) = dwa_control(
1505 DWAState::new(0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0),
1506 &legacy_config,
1507 (-5.0, -7.0),
1508 &obstacle_tuples,
1509 );
1510 assert!(legacy_u.0.abs() < 1.0e-15);
1511 assert_close(legacy_u.1, control[1]);
1512 assert_eq!(legacy_trajectory.len(), trajectory.len());
1513 assert_state_close(
1514 legacy_trajectory.last().unwrap(),
1515 &[
1516 -9.971_752_160_697_17e-18,
1517 -2.915_944_394_677_270_7e-18,
1518 0.183_259_571_459_404_3,
1519 -3.469_446_951_953_614e-18,
1520 -0.069_813_170_079_773_18,
1521 ],
1522 );
1523 }
1524
1525 #[test]
1526 fn test_dwa_rectangle_main_initial_parity_matches_pythonrobotics() {
1527 let mut dwa = DWAPlanner::new(DWAConfig {
1528 robot_type: DWARobotType::Rectangle,
1529 ..DWAConfig::default()
1530 });
1531 dwa.set_state(DWAState::new(
1532 0.0,
1533 0.0,
1534 std::f64::consts::FRAC_PI_8,
1535 0.0,
1536 0.0,
1537 ));
1538 dwa.set_goal(Point2D::new(1.0, 1.0));
1539 dwa.set_obstacles(pythonrobotics_default_obstacles());
1540
1541 let control = dwa.try_plan_step().unwrap();
1542 assert_close(control[0], 0.02);
1543 assert_close(control[1], 0.068_067_840_827_779_04);
1544
1545 let trajectory = &dwa.get_best_trajectory().states;
1546 assert_eq!(trajectory.len(), 31);
1547 assert_state_close(
1548 &trajectory[1],
1549 &[
1550 0.001_842_506_612_952_407,
1551 0.000_777_926_334_061_682_8,
1552 0.399_505_865_781_502_05,
1553 0.02,
1554 0.068_067_840_827_779_04,
1555 ],
1556 );
1557 assert_state_close(
1558 &trajectory[30],
1559 &[
1560 0.052_615_098_653_616_07,
1561 0.028_621_196_634_198_105,
1562 0.596_902_604_182_061_3,
1563 0.02,
1564 0.068_067_840_827_779_04,
1565 ],
1566 );
1567
1568 #[allow(deprecated)]
1569 let legacy_config = Config {
1570 max_speed: 1.0,
1571 min_speed: -0.5,
1572 max_yaw_rate: 40.0_f64.to_radians(),
1573 max_accel: 0.2,
1574 max_delta_yaw_rate: 40.0_f64.to_radians(),
1575 v_resolution: 0.01,
1576 yaw_rate_resolution: 0.1_f64.to_radians(),
1577 dt: 0.1,
1578 predict_time: 3.0,
1579 to_goal_cost_gain: 0.15,
1580 speed_cost_gain: 1.0,
1581 obstacle_cost_gain: 1.0,
1582 robot_type: DWARobotType::Rectangle,
1583 robot_radius: 1.0,
1584 robot_width: 0.5,
1585 robot_length: 1.2,
1586 };
1587 let obstacle_tuples: Vec<_> = pythonrobotics_default_obstacles()
1588 .into_iter()
1589 .map(|point| (point.x, point.y))
1590 .collect();
1591 #[allow(deprecated)]
1592 let (legacy_u, legacy_trajectory) = dwa_control(
1593 DWAState::new(0.0, 0.0, std::f64::consts::FRAC_PI_8, 0.0, 0.0),
1594 &legacy_config,
1595 (1.0, 1.0),
1596 &obstacle_tuples,
1597 );
1598 assert_close(legacy_u.0, control[0]);
1599 assert_close(legacy_u.1, control[1]);
1600 assert_eq!(legacy_trajectory.len(), trajectory.len());
1601 assert_state_close(
1602 legacy_trajectory.last().unwrap(),
1603 &[
1604 0.052_615_098_653_616_07,
1605 0.028_621_196_634_198_105,
1606 0.596_902_604_182_061_3,
1607 0.02,
1608 0.068_067_840_827_779_04,
1609 ],
1610 );
1611 }
1612
1613 #[test]
1614 fn test_dwa_default_closed_loop_matches_pythonrobotics_main() {
1615 let mut dwa = DWAPlanner::with_defaults();
1616 dwa.set_state(DWAState::new(
1617 0.0,
1618 0.0,
1619 std::f64::consts::FRAC_PI_8,
1620 0.0,
1621 0.0,
1622 ));
1623 dwa.set_goal(Point2D::new(10.0, 10.0));
1624 dwa.set_obstacles(pythonrobotics_default_obstacles());
1625
1626 let trace = run_closed_loop_trace(dwa, 1_000).unwrap();
1627 assert_eq!(trace.len(), 221);
1628 let expected = [
1629 (
1630 0,
1631 ClosedLoopSample {
1632 control: [0.02, 0.068_067_840_827_779_04],
1633 state: [
1634 0.001_842_506_612_952_407,
1635 0.000_777_926_334_061_682_8,
1636 0.399_505_865_781_502_05,
1637 0.02,
1638 0.068_067_840_827_779_04,
1639 ],
1640 predicted_len: 31,
1641 predicted_last: [
1642 0.052_615_098_653_616_07,
1643 0.028_621_196_634_198_105,
1644 0.596_902_604_182_061_3,
1645 0.02,
1646 0.068_067_840_827_779_04,
1647 ],
1648 dist: 14.140_282_717_861_751,
1649 },
1650 ),
1651 (
1652 1,
1653 ClosedLoopSample {
1654 control: [0.04, 0.129_154_364_647_580_6],
1655 state: [
1656 0.005_507_118_539_734_564,
1657 0.002_381_241_470_298_463_5,
1658 0.412_421_302_246_260_1,
1659 0.04,
1660 0.129_154_364_647_580_6,
1661 ],
1662 predicted_len: 31,
1663 predicted_last: [
1664 0.100_285_621_194_050_45,
1665 0.068_082_434_446_487_08,
1666 0.786_968_959_724_242_9,
1667 0.04,
1668 0.129_154_364_647_580_6,
1669 ],
1670 dist: 14.136_557_883_673_978,
1671 },
1672 ),
1673 (
1674 2,
1675 ClosedLoopSample {
1676 control: [0.06, 0.125_663_706_143_592],
1677 state: [
1678 0.010_973_381_433_457_923,
1679 0.004_855_098_777_562_393,
1680 0.424_987_672_860_619_34,
1681 0.06,
1682 0.125_663_706_143_592,
1683 ],
1684 predicted_len: 31,
1685 predicted_last: [
1686 0.152_459_040_871_135_35,
1687 0.104_477_455_550_390_23,
1688 0.789_412_420_677_035_5,
1689 0.06,
1690 0.125_663_706_143_592,
1691 ],
1692 dist: 14.130_943_860_296_941,
1693 },
1694 ),
1695 (
1696 10,
1697 ClosedLoopSample {
1698 control: [0.209_999_999_999_999_96, 0.097_738_438_111_683_48],
1699 state: [
1700 0.113_698_261_449_284_92,
1701 0.058_672_484_225_696_07,
1702 0.511_905_069_609_937_5,
1703 0.209_999_999_999_999_96,
1704 0.097_738_438_111_683_48,
1705 ],
1706 predicted_len: 31,
1707 predicted_last: [
1708 0.593_749_519_151_196_6,
1709 0.430_099_924_235_568_86,
1710 0.795_346_540_133_818_3,
1711 0.209_999_999_999_999_96,
1712 0.097_738_438_111_683_48,
1713 ],
1714 dist: 14.020_305_090_887_366,
1715 },
1716 ),
1717 (
1718 50,
1719 ClosedLoopSample {
1720 control: [0.990_000_000_000_000_7, 0.054_105_206_811_826_91],
1721 state: [
1722 1.950_167_034_188_733_6,
1723 1.694_578_822_612_822_2,
1724 0.921_882_910_903_411_1,
1725 0.990_000_000_000_000_7,
1726 0.054_105_206_811_826_91,
1727 ],
1728 predicted_len: 31,
1729 predicted_last: [
1730 3.492_440_509_739_950_5,
1731 4.112_668_327_825_561,
1732 1.078_788_010_657_709_2,
1733 0.990_000_000_000_000_7,
1734 0.054_105_206_811_826_91,
1735 ],
1736 dist: 11.566_323_171_658_883,
1737 },
1738 ),
1739 (
1740 100,
1741 ClosedLoopSample {
1742 control: [0.990_000_000_000_000_7, 0.078_539_816_339_751_13],
1743 state: [
1744 3.530_364_817_697_800_6,
1745 6.300_822_976_885_286,
1746 1.472_883_355_758_044_3,
1747 0.990_000_000_000_000_7,
1748 0.078_539_816_339_751_13,
1749 ],
1750 predicted_len: 31,
1751 predicted_last: [
1752 3.473_368_247_999_231,
1753 9.165_061_521_908_662,
1754 1.700_648_823_143_322_3,
1755 0.990_000_000_000_000_7,
1756 0.078_539_816_339_751_13,
1757 ],
1758 dist: 7.452_522_394_493_172,
1759 },
1760 ),
1761 (
1762 150,
1763 ClosedLoopSample {
1764 control: [0.990_000_000_000_000_7, -0.064_577_182_323_779_74],
1765 state: [
1766 5.167_079_306_155_943,
1767 10.598_045_278_751_194,
1768 0.561_297_887_441_443_9,
1769 0.990_000_000_000_000_7,
1770 -0.064_577_182_323_779_74,
1771 ],
1772 predicted_len: 31,
1773 predicted_last: [
1774 7.730_226_783_306_823,
1775 11.882_133_506_527_275,
1776 0.374_024_058_702_483_34,
1777 0.990_000_000_000_000_7,
1778 -0.064_577_182_323_779_74,
1779 ],
1780 dist: 4.869_782_396_413_901_5,
1781 },
1782 ),
1783 (
1784 200,
1785 ClosedLoopSample {
1786 control: [0.990_000_000_000_000_7, -0.383_972_435_438_737_94],
1787 state: [
1788 9.503_213_445_849_001,
1789 12.241_991_336_274_216,
1790 -0.511_730_536_684_608_7,
1791 0.990_000_000_000_000_7,
1792 -0.383_972_435_438_737_94,
1793 ],
1794 predicted_len: 31,
1795 predicted_last: [
1796 10.769_122_970_663_686,
1797 9.828_742_617_084_059,
1798 -1.625_250_599_456_947_5,
1799 0.990_000_000_000_000_7,
1800 -0.383_972_435_438_737_94,
1801 ],
1802 dist: 2.296_371_492_662_689_5,
1803 },
1804 ),
1805 (
1806 220,
1807 ClosedLoopSample {
1808 control: [0.990_000_000_000_000_7, -0.452_040_276_266_514_66],
1809 state: [
1810 10.682_812_904_798_725,
1811 10.723_560_741_508_503,
1812 -1.339_365_667_980_288_6,
1813 0.990_000_000_000_000_7,
1814 -0.452_040_276_266_514_66,
1815 ],
1816 predicted_len: 31,
1817 predicted_last: [
1818 9.529_554_489_209_27,
1819 8.315_446_240_490_422,
1820 -2.650_282_469_153_183,
1821 0.990_000_000_000_000_7,
1822 -0.452_040_276_266_514_66,
1823 ],
1824 dist: 0.994_873_665_151_514_7,
1825 },
1826 ),
1827 ];
1828 for (index, expected) in expected {
1829 assert_sample_close(&trace[index], &expected);
1830 }
1831 }
1832
1833 #[test]
1834 #[ignore = "long-running closed-loop regression scenario"]
1835 fn test_dwa_stuck_closed_loop_matches_pythonrobotics_reference() {
1836 let mut dwa = DWAPlanner::new(DWAConfig {
1837 to_goal_cost_gain: 0.2,
1838 obstacle_cost_gain: 2.0,
1839 ..DWAConfig::default()
1840 });
1841 dwa.set_state(DWAState::new(
1842 0.0,
1843 0.0,
1844 std::f64::consts::FRAC_PI_8,
1845 0.0,
1846 0.0,
1847 ));
1848 dwa.set_goal(Point2D::new(-5.0, -7.0));
1849 dwa.set_obstacles(pythonrobotics_stuck_obstacles());
1850
1851 let trace = run_closed_loop_trace(dwa, 1_000).unwrap();
1852 assert_eq!(trace.len(), 461);
1853 let expected = [
1854 (
1855 0,
1856 ClosedLoopSample {
1857 control: [-3.469_446_951_953_614e-18, -0.698_131_700_797_731_8],
1858 state: [
1859 -3.290_158_615_020_658_6e-19,
1860 -1.100_871_673_005_335_8e-19,
1861 0.322_885_911_618_950_97,
1862 -3.469_446_951_953_614e-18,
1863 -0.698_131_700_797_731_8,
1864 ],
1865 predicted_len: 31,
1866 predicted_last: [
1867 -9.971_752_160_697_17e-18,
1868 -2.915_944_394_677_270_7e-18,
1869 0.183_259_571_459_404_3,
1870 -3.469_446_951_953_614e-18,
1871 -0.069_813_170_079_773_18,
1872 ],
1873 dist: 8.602_325_267_042_627,
1874 },
1875 ),
1876 (
1877 1,
1878 ClosedLoopSample {
1879 control: [0.019_999_999_999_999_993, -0.698_131_700_797_731_8],
1880 state: [
1881 0.001_936_295_280_756_214_5,
1882 0.000_500_760_008_108_882_6,
1883 0.253_072_741_539_177_8,
1884 0.019_999_999_999_999_993,
1885 -0.698_131_700_797_731_8,
1886 ],
1887 predicted_len: 31,
1888 predicted_last: [
1889 0.036_000_096_958_749_65,
1890 -0.034_162_816_412_153_525,
1891 -1.771_509_190_774_245_5,
1892 0.019_999_999_999_999_993,
1893 -0.698_131_700_797_731_8,
1894 ],
1895 dist: 8.603_858_296_887_57,
1896 },
1897 ),
1898 (
1899 2,
1900 ClosedLoopSample {
1901 control: [0.039_999_999_999_999_994, -0.698_131_700_797_731_8],
1902 state: [
1903 0.005_869_314_911_012_033,
1904 0.001_229_702_110_077_472_6,
1905 0.183_259_571_459_404_64,
1906 0.039_999_999_999_999_994,
1907 -0.698_131_700_797_731_8,
1908 ],
1909 predicted_len: 31,
1910 predicted_last: [
1911 0.068_994_945_132_430_06,
1912 -0.072_680_914_645_250_44,
1913 -1.841_322_360_854_018_8,
1914 0.039_999_999_999_999_994,
1915 -0.698_131_700_797_731_8,
1916 ],
1917 dist: 8.606_738_345_022_23,
1918 },
1919 ),
1920 (
1921 10,
1922 ClosedLoopSample {
1923 control: [0.189_999_999_999_999_95, -0.647_517_152_489_896_2],
1924 state: [
1925 0.104_132_970_400_981_02,
1926 -0.016_809_261_291_929_67,
1927 -0.366_344_609_993_609_74,
1928 0.189_999_999_999_999_95,
1929 -0.647_517_152_489_896_2,
1930 ],
1931 predicted_len: 31,
1932 predicted_last: [
1933 0.213_572_392_076_469_75,
1934 -0.477_614_640_782_877_6,
1935 -2.244_144_352_214_308_3,
1936 0.189_999_999_999_999_95,
1937 -0.647_517_152_489_896_2,
1938 ],
1939 dist: 8.649_689_374_348_22,
1940 },
1941 ),
1942 (
1943 50,
1944 ClosedLoopSample {
1945 control: [0.029_999_999_999_999_79, -0.167_551_608_191_450_54],
1946 state: [
1947 0.343_298_669_611_588_45,
1948 -0.402_879_787_203_639_7,
1949 -1.769_938_394_447_439,
1950 0.029_999_999_999_999_79,
1951 -0.167_551_608_191_450_54,
1952 ],
1953 predicted_len: 31,
1954 predicted_last: [
1955 0.305_790_986_927_416_24,
1956 -0.480_433_609_151_910_36,
1957 -2.255_838_058_202_643,
1958 0.029_999_999_999_999_79,
1959 -0.167_551_608_191_450_54,
1960 ],
1961 dist: 8.489_572_178_547,
1962 },
1963 ),
1964 (
1965 100,
1966 ClosedLoopSample {
1967 control: [0.269_999_999_999_999_7, -0.005_235_987_755_980_706],
1968 state: [
1969 0.106_092_820_485_199_51,
1970 -0.538_702_831_822_151_7,
1971 -2.837_905_363_742_752,
1972 0.269_999_999_999_999_7,
1973 -0.005_235_987_755_980_706,
1974 ],
1975 predicted_len: 31,
1976 predicted_last: [
1977 -0.642_886_107_988_420_7,
1978 -0.766_974_159_554_404_4,
1979 -2.853_089_728_235_101_8,
1980 0.269_999_999_999_999_7,
1981 -0.005_235_987_755_980_706,
1982 ],
1983 dist: 8.235_323_004_406_906,
1984 },
1985 ),
1986 (
1987 200,
1988 ClosedLoopSample {
1989 control: [0.990_000_000_000_000_3, 0.308_923_277_603_009_64],
1990 state: [
1991 1.852_193_706_436_120_7,
1992 -4.220_215_843_560_918,
1993 0.757_647_428_290_844_2,
1994 0.990_000_000_000_000_3,
1995 0.308_923_277_603_009_64,
1996 ],
1997 predicted_len: 31,
1998 predicted_last: [
1999 2.803_482_546_405_410_5,
2000 -1.612_237_727_831_649_8,
2001 1.653_524_933_339_573_6,
2002 0.990_000_000_000_000_3,
2003 0.308_923_277_603_009_64,
2004 ],
2005 dist: 7.394_576_292_588_543,
2006 },
2007 ),
2008 (
2009 300,
2010 ClosedLoopSample {
2011 control: [0.990_000_000_000_000_3, 0.371_755_130_674_814_8],
2012 state: [
2013 0.769_215_774_844_203_3,
2014 3.386_587_574_598_417_3,
2015 3.243_519_881_906_541,
2016 0.990_000_000_000_000_3,
2017 0.371_755_130_674_814_8,
2018 ],
2019 predicted_len: 31,
2020 predicted_last: [
2021 -1.391_460_065_500_792_3,
2022 1.711_199_521_041_678_8,
2023 4.321_609_760_863_501,
2024 0.990_000_000_000_000_3,
2025 0.371_755_130_674_814_8,
2026 ],
2027 dist: 11.881_290_001_574_468,
2028 },
2029 ),
2030 (
2031 400,
2032 ClosedLoopSample {
2033 control: [0.299_999_999_999_999_7, 0.118_682_389_135_646_52],
2034 state: [
2035 -4.821_582_173_842_302,
2036 -2.257_266_302_545_886_6,
2037 4.841_543_345_032_829,
2038 0.299_999_999_999_999_7,
2039 0.118_682_389_135_646_52,
2040 ],
2041 predicted_len: 31,
2042 predicted_last: [
2043 -4.559_815_331_068_457,
2044 -3.082_459_945_503_760_2,
2045 5.185_722_273_526_192,
2046 0.299_999_999_999_999_7,
2047 0.118_682_389_135_646_52,
2048 ],
2049 dist: 4.746_088_478_490_219,
2050 },
2051 ),
2052 (
2053 460,
2054 ClosedLoopSample {
2055 control: [0.990_000_000_000_000_3, -0.506_145_483_078_318_9],
2056 state: [
2057 -4.985_352_164_947_927,
2058 -6.059_407_170_506_825,
2059 4.171_685_978_117_614,
2060 0.990_000_000_000_000_3,
2061 -0.506_145_483_078_318_9,
2062 ],
2063 predicted_len: 31,
2064 predicted_last: [
2065 -7.510_205_006_513_761,
2066 -6.760_560_048_330_578_6,
2067 2.703_864_077_190_488,
2068 0.990_000_000_000_000_3,
2069 -0.506_145_483_078_318_9,
2070 ],
2071 dist: 0.940_706_877_813_535_5,
2072 },
2073 ),
2074 ];
2075 for (index, expected) in expected {
2076 assert_sample_close(&trace[index], &expected);
2077 }
2078 }
2079
2080 #[test]
2081 fn test_dwa_rectangle_closed_loop_matches_pythonrobotics_main2() {
2082 let mut dwa = DWAPlanner::new(DWAConfig {
2083 robot_type: DWARobotType::Rectangle,
2084 ..DWAConfig::default()
2085 });
2086 dwa.set_state(DWAState::new(
2087 0.0,
2088 0.0,
2089 std::f64::consts::FRAC_PI_8,
2090 0.0,
2091 0.0,
2092 ));
2093 dwa.set_goal(Point2D::new(1.0, 1.0));
2094 dwa.set_obstacles(pythonrobotics_default_obstacles());
2095
2096 let trace = run_closed_loop_trace(dwa, 1_000).unwrap();
2097 assert!((21..=22).contains(&trace.len()));
2098 assert!(trace.last().unwrap().dist <= 1.05);
2099 let expected = [
2100 (
2101 0,
2102 ClosedLoopSample {
2103 control: [0.02, 0.068_067_840_827_779_04],
2104 state: [
2105 0.001_842_506_612_952_407,
2106 0.000_777_926_334_061_682_8,
2107 0.399_505_865_781_502_05,
2108 0.02,
2109 0.068_067_840_827_779_04,
2110 ],
2111 predicted_len: 31,
2112 predicted_last: [
2113 0.052_615_098_653_616_07,
2114 0.028_621_196_634_198_105,
2115 0.596_902_604_182_061_3,
2116 0.02,
2117 0.068_067_840_827_779_04,
2118 ],
2119 dist: 1.412_360_837_075_983_3,
2120 },
2121 ),
2122 (
2123 1,
2124 ClosedLoopSample {
2125 control: [0.04, 0.134_390_352_403_563_56],
2126 state: [
2127 0.005_506_278_543_593_619,
2128 0.002_383_160_036_749_725_7,
2129 0.412_944_901_021_858_4,
2130 0.04,
2131 0.134_390_352_403_563_56,
2132 ],
2133 predicted_len: 31,
2134 predicted_last: [
2135 0.099_685_436_565_874_95,
2136 0.068_843_867_784_433_95,
2137 0.802_676_922_992_193_3,
2138 0.04,
2139 0.134_390_352_403_563_56,
2140 ],
2141 dist: 1.408_636_617_937_526_7,
2142 },
2143 ),
2144 (
2145 2,
2146 ClosedLoopSample {
2147 control: [0.06, 0.132_645_023_151_569_34],
2148 state: [
2149 0.010_969_514_971_661_525,
2150 0.004_863_693_796_050_7,
2151 0.426_209_403_337_015_3,
2152 0.06,
2153 0.132_645_023_151_569_34,
2154 ],
2155 predicted_len: 31,
2156 predicted_last: [
2157 0.151_191_833_116_684_02,
2158 0.106_069_645_038_751_56,
2159 0.810_879_970_476_567,
2160 0.06,
2161 0.132_645_023_151_569_34,
2162 ],
2163 dist: 1.403_024_436_081_079_3,
2164 },
2165 ),
2166 (
2167 5,
2168 ClosedLoopSample {
2169 control: [0.119_999_999_999_999_95, 0.129_154_364_647_581],
2170 state: [
2171 0.037_928_184_668_002_49,
2172 0.018_021_637_577_041_828,
2173 0.465_304_778_581_688_45,
2174 0.119_999_999_999_999_95,
2175 0.129_154_364_647_581,
2176 ],
2177 predicted_len: 31,
2178 predicted_last: [
2179 0.311_447_525_499_293_9,
2180 0.229_879_307_030_410_07,
2181 0.839_852_436_059_674,
2182 0.119_999_999_999_999_95,
2183 0.129_154_364_647_581,
2184 ],
2185 dist: 1.374_723_129_260_244_9,
2186 },
2187 ),
2188 (
2189 10,
2190 ClosedLoopSample {
2191 control: [0.209_999_999_999_999_96, 0.127_409_035_395_587_23],
2192 state: [
2193 0.113_124_079_420_109_7,
2194 0.059_725_368_755_827_526,
2195 0.529_183_829_204_681_3,
2196 0.209_999_999_999_999_96,
2197 0.127_409_035_395_587_23,
2198 ],
2199 predicted_len: 31,
2200 predicted_last: [
2201 0.568_258_124_617_134_2,
2202 0.459_148_358_042_203_83,
2203 0.898_670_031_851_883_7,
2204 0.209_999_999_999_999_96,
2205 0.127_409_035_395_587_23,
2206 ],
2207 dist: 1.292_542_177_519_090_6,
2208 },
2209 ),
2210 (
2211 20,
2212 ClosedLoopSample {
2213 control: [0.340_000_000_000_000_1, 0.139_626_340_159_548_67],
2214 state: [
2215 0.365_710_870_049_402_1,
2216 0.235_568_742_831_355_18,
2217 0.666_192_175_486_237_9,
2218 0.340_000_000_000_000_1,
2219 0.139_626_340_159_548_67,
2220 ],
2221 predicted_len: 31,
2222 predicted_last: [
2223 0.992_955_085_115_716,
2224 0.987_609_593_409_779_3,
2225 1.071_108_561_948_929_3,
2226 0.340_000_000_000_000_1,
2227 0.139_626_340_159_548_67,
2228 ],
2229 dist: 0.993_316_589_668_128_5,
2230 },
2231 ),
2232 ];
2233 for (index, expected) in expected {
2234 assert_sample_close(&trace[index], &expected);
2235 }
2236 }
2237}