rust_robotics_planning/
stl_cbs.rs

1//! STL-CBS foundation for multi-agent grid planning.
2//!
3//! This module combines a compact conflict-based search (CBS) planner with
4//! Signal Temporal Logic style robustness primitives over timed grid paths.
5
6use std::cmp::Ordering;
7use std::collections::{BinaryHeap, HashMap, HashSet};
8
9use rust_robotics_core::{RoboticsError, RoboticsResult};
10
11/// Timed grid cell used by STL-CBS paths.
12#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
13pub struct StlTimedCell {
14    pub x: i32,
15    pub y: i32,
16    pub t: u64,
17}
18
19impl StlTimedCell {
20    pub fn new(x: i32, y: i32, t: u64) -> Self {
21        Self { x, y, t }
22    }
23}
24
25/// Agent start/goal query for STL-CBS.
26#[derive(Debug, Clone, Copy, PartialEq, Eq)]
27pub struct StlCbsAgent {
28    pub id: usize,
29    pub start: (i32, i32),
30    pub goal: (i32, i32),
31}
32
33impl StlCbsAgent {
34    pub fn new(id: usize, start: (i32, i32), goal: (i32, i32)) -> Self {
35        Self { id, start, goal }
36    }
37}
38
39/// Configuration for the grid CBS planner.
40#[derive(Debug, Clone, PartialEq)]
41pub struct StlCbsConfig {
42    pub width: i32,
43    pub height: i32,
44    pub obstacle_map: Vec<Vec<bool>>,
45    pub max_time: u64,
46    pub max_cbs_nodes: usize,
47    pub allow_wait: bool,
48}
49
50impl StlCbsConfig {
51    pub fn new(width: i32, height: i32) -> Self {
52        Self {
53            width,
54            height,
55            obstacle_map: vec![vec![false; height.max(0) as usize]; width.max(0) as usize],
56            max_time: 64,
57            max_cbs_nodes: 512,
58            allow_wait: true,
59        }
60    }
61}
62
63/// One agent's planned timed path.
64#[derive(Debug, Clone, PartialEq, Eq)]
65pub struct StlCbsPath {
66    pub agent_id: usize,
67    pub waypoints: Vec<StlTimedCell>,
68}
69
70impl StlCbsPath {
71    pub fn arrival_time(&self) -> u64 {
72        self.waypoints.last().map_or(0, |waypoint| waypoint.t)
73    }
74
75    pub fn position_at(&self, t: u64) -> StlTimedCell {
76        self.waypoints
77            .iter()
78            .find(|waypoint| waypoint.t == t)
79            .copied()
80            .or_else(|| {
81                self.waypoints
82                    .iter()
83                    .rev()
84                    .find(|waypoint| waypoint.t < t)
85                    .copied()
86            })
87            .unwrap_or_else(|| {
88                *self
89                    .waypoints
90                    .first()
91                    .expect("validated STL-CBS path is non-empty")
92            })
93    }
94}
95
96/// CBS result with robustness summary.
97#[derive(Debug, Clone, PartialEq)]
98pub struct StlCbsPlan {
99    pub paths: Vec<StlCbsPath>,
100    pub total_cost: u64,
101    pub conflicts_resolved: usize,
102    pub high_level_nodes_expanded: usize,
103    pub min_pairwise_separation_robustness: f64,
104}
105
106/// Axis-aligned rectangle used by STL predicates.
107#[derive(Debug, Clone, Copy, PartialEq)]
108pub struct StlRectangle2D {
109    pub min_x: f64,
110    pub max_x: f64,
111    pub min_y: f64,
112    pub max_y: f64,
113}
114
115impl StlRectangle2D {
116    pub fn new(min_x: f64, max_x: f64, min_y: f64, max_y: f64) -> RoboticsResult<Self> {
117        if !min_x.is_finite()
118            || !max_x.is_finite()
119            || !min_y.is_finite()
120            || !max_y.is_finite()
121            || min_x > max_x
122            || min_y > max_y
123        {
124            return Err(RoboticsError::InvalidParameter(
125                "STL rectangle bounds must be finite and ordered".to_string(),
126            ));
127        }
128        Ok(Self {
129            min_x,
130            max_x,
131            min_y,
132            max_y,
133        })
134    }
135
136    pub fn inside_robustness(&self, x: f64, y: f64) -> f64 {
137        (x - self.min_x)
138            .min(self.max_x - x)
139            .min(y - self.min_y)
140            .min(self.max_y - y)
141    }
142
143    pub fn avoid_robustness(&self, x: f64, y: f64) -> f64 {
144        -self.inside_robustness(x, y)
145    }
146}
147
148/// Closed integer time interval for STL temporal operators.
149#[derive(Debug, Clone, Copy, PartialEq, Eq)]
150pub struct StlTimeInterval {
151    pub start: u64,
152    pub end: u64,
153}
154
155impl StlTimeInterval {
156    pub fn new(start: u64, end: u64) -> RoboticsResult<Self> {
157        if start > end {
158            return Err(RoboticsError::InvalidParameter(
159                "STL time interval start must be <= end".to_string(),
160            ));
161        }
162        Ok(Self { start, end })
163    }
164}
165
166/// A high-level CBS conflict.
167#[derive(Debug, Clone, Copy, PartialEq, Eq)]
168pub struct StlCbsConflict {
169    pub agent_a: usize,
170    pub agent_b: usize,
171    pub kind: StlCbsConflictKind,
172}
173
174#[derive(Debug, Clone, Copy, PartialEq, Eq)]
175pub enum StlCbsConflictKind {
176    Vertex {
177        x: i32,
178        y: i32,
179        t: u64,
180    },
181    Edge {
182        ax0: i32,
183        ay0: i32,
184        ax1: i32,
185        ay1: i32,
186        bx0: i32,
187        by0: i32,
188        bx1: i32,
189        by1: i32,
190        t: u64,
191    },
192}
193
194#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
195enum StlCbsConstraintKind {
196    Vertex {
197        x: i32,
198        y: i32,
199        t: u64,
200    },
201    Edge {
202        from_x: i32,
203        from_y: i32,
204        to_x: i32,
205        to_y: i32,
206        t: u64,
207    },
208}
209
210#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
211struct StlCbsConstraint {
212    agent_id: usize,
213    kind: StlCbsConstraintKind,
214}
215
216#[derive(Debug, Clone, PartialEq, Eq)]
217struct CbsNode {
218    constraints: Vec<StlCbsConstraint>,
219    paths: Vec<StlCbsPath>,
220    total_cost: u64,
221    conflicts_resolved: usize,
222}
223
224#[derive(Debug, Clone, Copy, PartialEq, Eq)]
225struct CbsOpenEntry {
226    node_index: usize,
227    total_cost: u64,
228    conflicts_resolved: usize,
229}
230
231impl Ord for CbsOpenEntry {
232    fn cmp(&self, other: &Self) -> Ordering {
233        other
234            .total_cost
235            .cmp(&self.total_cost)
236            .then_with(|| other.conflicts_resolved.cmp(&self.conflicts_resolved))
237            .then_with(|| other.node_index.cmp(&self.node_index))
238    }
239}
240
241impl PartialOrd for CbsOpenEntry {
242    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
243        Some(self.cmp(other))
244    }
245}
246
247#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
248struct SearchState {
249    x: i32,
250    y: i32,
251    t: u64,
252}
253
254#[derive(Debug, Clone, Copy, PartialEq, Eq)]
255struct SearchOpenEntry {
256    state: SearchState,
257    f: u64,
258    g: u64,
259}
260
261impl Ord for SearchOpenEntry {
262    fn cmp(&self, other: &Self) -> Ordering {
263        other
264            .f
265            .cmp(&self.f)
266            .then_with(|| other.g.cmp(&self.g))
267            .then_with(|| other.state.t.cmp(&self.state.t))
268    }
269}
270
271impl PartialOrd for SearchOpenEntry {
272    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
273        Some(self.cmp(other))
274    }
275}
276
277/// Conflict-based multi-agent grid planner.
278#[derive(Debug, Clone, PartialEq)]
279pub struct StlCbsPlanner {
280    config: StlCbsConfig,
281    motions: Vec<(i32, i32)>,
282}
283
284impl StlCbsPlanner {
285    pub fn new(config: StlCbsConfig) -> RoboticsResult<Self> {
286        validate_config(&config)?;
287        let mut motions = vec![(1, 0), (0, 1), (-1, 0), (0, -1)];
288        if config.allow_wait {
289            motions.push((0, 0));
290        }
291        Ok(Self { config, motions })
292    }
293
294    pub fn plan_independent(&self, agents: &[StlCbsAgent]) -> RoboticsResult<Vec<StlCbsPath>> {
295        self.validate_agents(agents)?;
296        agents
297            .iter()
298            .map(|agent| self.plan_agent(*agent, &[]))
299            .collect()
300    }
301
302    pub fn plan(&self, agents: &[StlCbsAgent]) -> RoboticsResult<StlCbsPlan> {
303        self.validate_agents(agents)?;
304        let root_paths = self.plan_independent(agents)?;
305        let root = CbsNode {
306            total_cost: total_path_cost(&root_paths),
307            constraints: Vec::new(),
308            paths: root_paths,
309            conflicts_resolved: 0,
310        };
311
312        let mut nodes = vec![root];
313        let mut open = BinaryHeap::new();
314        open.push(CbsOpenEntry {
315            node_index: 0,
316            total_cost: nodes[0].total_cost,
317            conflicts_resolved: 0,
318        });
319        let mut expanded = 0;
320
321        while let Some(entry) = open.pop() {
322            expanded += 1;
323            if expanded > self.config.max_cbs_nodes {
324                return Err(RoboticsError::PlanningError(
325                    "STL-CBS exceeded high-level node limit".to_string(),
326                ));
327            }
328            let node = nodes[entry.node_index].clone();
329            if let Some(conflict) = first_conflict(&node.paths, self.config.max_time) {
330                for constrained_agent in [conflict.agent_a, conflict.agent_b] {
331                    let mut constraints = node.constraints.clone();
332                    constraints.push(constraint_from_conflict(conflict, constrained_agent));
333                    let agent = *agents
334                        .iter()
335                        .find(|agent| agent.id == constrained_agent)
336                        .expect("validated conflict references a known agent");
337                    let agent_constraints = constraints_for_agent(&constraints, constrained_agent);
338                    let Ok(replanned_path) = self.plan_agent(agent, &agent_constraints) else {
339                        continue;
340                    };
341                    let mut paths = node.paths.clone();
342                    let path_index = paths
343                        .iter()
344                        .position(|path| path.agent_id == constrained_agent)
345                        .expect("validated node contains all agent paths");
346                    paths[path_index] = replanned_path;
347                    let child = CbsNode {
348                        total_cost: total_path_cost(&paths),
349                        constraints,
350                        paths,
351                        conflicts_resolved: node.conflicts_resolved + 1,
352                    };
353                    let node_index = nodes.len();
354                    open.push(CbsOpenEntry {
355                        node_index,
356                        total_cost: child.total_cost,
357                        conflicts_resolved: child.conflicts_resolved,
358                    });
359                    nodes.push(child);
360                }
361            } else {
362                return Ok(StlCbsPlan {
363                    total_cost: node.total_cost,
364                    min_pairwise_separation_robustness: stl_pairwise_separation_robustness(
365                        &node.paths,
366                        1.0,
367                        StlTimeInterval {
368                            start: 0,
369                            end: self.config.max_time,
370                        },
371                    )?,
372                    paths: node.paths,
373                    conflicts_resolved: node.conflicts_resolved,
374                    high_level_nodes_expanded: expanded,
375                });
376            }
377        }
378
379        Err(RoboticsError::PlanningError(
380            "STL-CBS could not find a conflict-free plan".to_string(),
381        ))
382    }
383
384    fn plan_agent(
385        &self,
386        agent: StlCbsAgent,
387        constraints: &[StlCbsConstraintKind],
388    ) -> RoboticsResult<StlCbsPath> {
389        if self.violates_vertex_constraint(agent.start.0, agent.start.1, 0, constraints) {
390            return Err(RoboticsError::PlanningError(
391                "agent start violates a CBS vertex constraint".to_string(),
392            ));
393        }
394        let start = SearchState {
395            x: agent.start.0,
396            y: agent.start.1,
397            t: 0,
398        };
399        let mut open = BinaryHeap::new();
400        let mut best_g = HashMap::new();
401        let mut parent: HashMap<SearchState, Option<SearchState>> = HashMap::new();
402
403        best_g.insert(start, 0);
404        parent.insert(start, None);
405        open.push(SearchOpenEntry {
406            state: start,
407            g: 0,
408            f: manhattan(agent.start, agent.goal),
409        });
410
411        while let Some(entry) = open.pop() {
412            if entry.state.t > self.config.max_time {
413                continue;
414            }
415            if entry.g > *best_g.get(&entry.state).unwrap_or(&u64::MAX) {
416                continue;
417            }
418            if (entry.state.x, entry.state.y) == agent.goal {
419                return Ok(StlCbsPath {
420                    agent_id: agent.id,
421                    waypoints: reconstruct_timed_path(&parent, entry.state, agent.id),
422                });
423            }
424            let next_t = entry.state.t + 1;
425            if next_t > self.config.max_time {
426                continue;
427            }
428            for &(dx, dy) in &self.motions {
429                let nx = entry.state.x + dx;
430                let ny = entry.state.y + dy;
431                if !self.is_free(nx, ny) {
432                    continue;
433                }
434                if self.violates_vertex_constraint(nx, ny, next_t, constraints)
435                    || self.violates_edge_constraint(
436                        entry.state.x,
437                        entry.state.y,
438                        nx,
439                        ny,
440                        next_t,
441                        constraints,
442                    )
443                {
444                    continue;
445                }
446                let next = SearchState {
447                    x: nx,
448                    y: ny,
449                    t: next_t,
450                };
451                let next_g = entry.g + 1;
452                if next_g < *best_g.get(&next).unwrap_or(&u64::MAX) {
453                    best_g.insert(next, next_g);
454                    parent.insert(next, Some(entry.state));
455                    open.push(SearchOpenEntry {
456                        state: next,
457                        g: next_g,
458                        f: next_g + manhattan((nx, ny), agent.goal),
459                    });
460                }
461            }
462        }
463
464        Err(RoboticsError::PlanningError(format!(
465            "no path found for agent {}",
466            agent.id
467        )))
468    }
469
470    fn validate_agents(&self, agents: &[StlCbsAgent]) -> RoboticsResult<()> {
471        if agents.is_empty() {
472            return Err(RoboticsError::InvalidParameter(
473                "STL-CBS requires at least one agent".to_string(),
474            ));
475        }
476        let mut ids = HashSet::new();
477        for agent in agents {
478            if !ids.insert(agent.id) {
479                return Err(RoboticsError::InvalidParameter(
480                    "STL-CBS agent ids must be unique".to_string(),
481                ));
482            }
483            if !self.is_free(agent.start.0, agent.start.1)
484                || !self.is_free(agent.goal.0, agent.goal.1)
485            {
486                return Err(RoboticsError::InvalidParameter(format!(
487                    "agent {} start/goal must be free grid cells",
488                    agent.id
489                )));
490            }
491        }
492        Ok(())
493    }
494
495    fn is_free(&self, x: i32, y: i32) -> bool {
496        x >= 0
497            && y >= 0
498            && x < self.config.width
499            && y < self.config.height
500            && !self.config.obstacle_map[x as usize][y as usize]
501    }
502
503    fn violates_vertex_constraint(
504        &self,
505        x: i32,
506        y: i32,
507        t: u64,
508        constraints: &[StlCbsConstraintKind],
509    ) -> bool {
510        constraints.iter().any(|constraint| {
511            matches!(
512                constraint,
513                StlCbsConstraintKind::Vertex { x: cx, y: cy, t: ct }
514                    if *cx == x && *cy == y && *ct == t
515            )
516        })
517    }
518
519    fn violates_edge_constraint(
520        &self,
521        from_x: i32,
522        from_y: i32,
523        to_x: i32,
524        to_y: i32,
525        t: u64,
526        constraints: &[StlCbsConstraintKind],
527    ) -> bool {
528        constraints.iter().any(|constraint| {
529            matches!(
530                constraint,
531                StlCbsConstraintKind::Edge {
532                    from_x: cx0,
533                    from_y: cy0,
534                    to_x: cx1,
535                    to_y: cy1,
536                    t: ct,
537                } if *cx0 == from_x
538                    && *cy0 == from_y
539                    && *cx1 == to_x
540                    && *cy1 == to_y
541                    && *ct == t
542            )
543        })
544    }
545}
546
547/// Robustness of `F_[interval] inside(region)` for one path.
548pub fn stl_eventually_reach_robustness(
549    path: &StlCbsPath,
550    region: StlRectangle2D,
551    interval: StlTimeInterval,
552) -> RoboticsResult<f64> {
553    validate_path(path)?;
554    let mut robustness = f64::NEG_INFINITY;
555    for t in interval.start..=interval.end {
556        let position = path.position_at(t);
557        robustness = robustness.max(region.inside_robustness(position.x as f64, position.y as f64));
558    }
559    Ok(robustness)
560}
561
562/// Robustness of `G_[interval] outside(region)` for one path.
563pub fn stl_always_avoid_robustness(
564    path: &StlCbsPath,
565    region: StlRectangle2D,
566    interval: StlTimeInterval,
567) -> RoboticsResult<f64> {
568    validate_path(path)?;
569    let mut robustness = f64::INFINITY;
570    for t in interval.start..=interval.end {
571        let position = path.position_at(t);
572        robustness = robustness.min(region.avoid_robustness(position.x as f64, position.y as f64));
573    }
574    Ok(robustness)
575}
576
577/// Robustness of pairwise Euclidean separation over all paths.
578pub fn stl_pairwise_separation_robustness(
579    paths: &[StlCbsPath],
580    min_distance: f64,
581    interval: StlTimeInterval,
582) -> RoboticsResult<f64> {
583    if paths.len() < 2 {
584        return Ok(f64::INFINITY);
585    }
586    if min_distance < 0.0 || !min_distance.is_finite() {
587        return Err(RoboticsError::InvalidParameter(
588            "STL min_distance must be finite and non-negative".to_string(),
589        ));
590    }
591    for path in paths {
592        validate_path(path)?;
593    }
594    let mut robustness = f64::INFINITY;
595    for t in interval.start..=interval.end {
596        for i in 0..paths.len() {
597            for j in i + 1..paths.len() {
598                let a = paths[i].position_at(t);
599                let b = paths[j].position_at(t);
600                robustness = robustness.min(euclidean((a.x, a.y), (b.x, b.y)) - min_distance);
601            }
602        }
603    }
604    Ok(robustness)
605}
606
607pub fn first_conflict(paths: &[StlCbsPath], max_time: u64) -> Option<StlCbsConflict> {
608    for t in 0..=max_time {
609        for i in 0..paths.len() {
610            for j in i + 1..paths.len() {
611                let a = paths[i].position_at(t);
612                let b = paths[j].position_at(t);
613                if a.x == b.x && a.y == b.y {
614                    return Some(StlCbsConflict {
615                        agent_a: paths[i].agent_id,
616                        agent_b: paths[j].agent_id,
617                        kind: StlCbsConflictKind::Vertex { x: a.x, y: a.y, t },
618                    });
619                }
620                if t > 0 {
621                    let a_prev = paths[i].position_at(t - 1);
622                    let b_prev = paths[j].position_at(t - 1);
623                    if a_prev.x == b.x && a_prev.y == b.y && b_prev.x == a.x && b_prev.y == a.y {
624                        return Some(StlCbsConflict {
625                            agent_a: paths[i].agent_id,
626                            agent_b: paths[j].agent_id,
627                            kind: StlCbsConflictKind::Edge {
628                                ax0: a_prev.x,
629                                ay0: a_prev.y,
630                                ax1: a.x,
631                                ay1: a.y,
632                                bx0: b_prev.x,
633                                by0: b_prev.y,
634                                bx1: b.x,
635                                by1: b.y,
636                                t,
637                            },
638                        });
639                    }
640                }
641            }
642        }
643    }
644    None
645}
646
647fn constraint_from_conflict(
648    conflict: StlCbsConflict,
649    constrained_agent: usize,
650) -> StlCbsConstraint {
651    let kind = match conflict.kind {
652        StlCbsConflictKind::Vertex { x, y, t } => StlCbsConstraintKind::Vertex { x, y, t },
653        StlCbsConflictKind::Edge {
654            ax0,
655            ay0,
656            ax1,
657            ay1,
658            bx0,
659            by0,
660            bx1,
661            by1,
662            t,
663        } => {
664            if constrained_agent == conflict.agent_a {
665                StlCbsConstraintKind::Edge {
666                    from_x: ax0,
667                    from_y: ay0,
668                    to_x: ax1,
669                    to_y: ay1,
670                    t,
671                }
672            } else {
673                StlCbsConstraintKind::Edge {
674                    from_x: bx0,
675                    from_y: by0,
676                    to_x: bx1,
677                    to_y: by1,
678                    t,
679                }
680            }
681        }
682    };
683    StlCbsConstraint {
684        agent_id: constrained_agent,
685        kind,
686    }
687}
688
689fn constraints_for_agent(
690    constraints: &[StlCbsConstraint],
691    agent_id: usize,
692) -> Vec<StlCbsConstraintKind> {
693    constraints
694        .iter()
695        .filter(|constraint| constraint.agent_id == agent_id)
696        .map(|constraint| constraint.kind)
697        .collect()
698}
699
700fn total_path_cost(paths: &[StlCbsPath]) -> u64 {
701    paths.iter().map(StlCbsPath::arrival_time).sum()
702}
703
704fn reconstruct_timed_path(
705    parent: &HashMap<SearchState, Option<SearchState>>,
706    mut current: SearchState,
707    agent_id: usize,
708) -> Vec<StlTimedCell> {
709    let mut reversed = Vec::new();
710    loop {
711        reversed.push(StlTimedCell::new(current.x, current.y, current.t));
712        match parent.get(&current).copied().flatten() {
713            Some(previous) => current = previous,
714            None => break,
715        }
716    }
717    reversed.reverse();
718    let _ = agent_id;
719    reversed
720}
721
722fn manhattan(a: (i32, i32), b: (i32, i32)) -> u64 {
723    (a.0 - b.0).unsigned_abs() as u64 + (a.1 - b.1).unsigned_abs() as u64
724}
725
726fn euclidean(a: (i32, i32), b: (i32, i32)) -> f64 {
727    let dx = (a.0 - b.0) as f64;
728    let dy = (a.1 - b.1) as f64;
729    (dx * dx + dy * dy).sqrt()
730}
731
732fn validate_config(config: &StlCbsConfig) -> RoboticsResult<()> {
733    if config.width <= 0 || config.height <= 0 {
734        return Err(RoboticsError::InvalidParameter(
735            "STL-CBS width and height must be positive".to_string(),
736        ));
737    }
738    if config.max_time == 0 || config.max_cbs_nodes == 0 {
739        return Err(RoboticsError::InvalidParameter(
740            "STL-CBS max_time and max_cbs_nodes must be positive".to_string(),
741        ));
742    }
743    if config.obstacle_map.len() != config.width as usize {
744        return Err(RoboticsError::InvalidParameter(
745            "STL-CBS obstacle_map x-dimension must match width".to_string(),
746        ));
747    }
748    for column in &config.obstacle_map {
749        if column.len() != config.height as usize {
750            return Err(RoboticsError::InvalidParameter(
751                "STL-CBS obstacle_map y-dimension must match height".to_string(),
752            ));
753        }
754    }
755    Ok(())
756}
757
758fn validate_path(path: &StlCbsPath) -> RoboticsResult<()> {
759    if path.waypoints.is_empty() {
760        return Err(RoboticsError::InvalidParameter(
761            "STL-CBS path must contain at least one waypoint".to_string(),
762        ));
763    }
764    for window in path.waypoints.windows(2) {
765        if window[1].t <= window[0].t {
766            return Err(RoboticsError::InvalidParameter(
767                "STL-CBS path times must be strictly increasing".to_string(),
768            ));
769        }
770    }
771    Ok(())
772}
773
774#[cfg(test)]
775mod tests {
776    use super::*;
777
778    fn corridor_planner() -> StlCbsPlanner {
779        StlCbsPlanner::new(StlCbsConfig {
780            max_time: 12,
781            ..StlCbsConfig::new(5, 3)
782        })
783        .unwrap()
784    }
785
786    #[test]
787    fn independent_paths_have_a_conflict() {
788        let planner = corridor_planner();
789        let agents = [
790            StlCbsAgent::new(0, (0, 1), (4, 1)),
791            StlCbsAgent::new(1, (4, 1), (0, 1)),
792        ];
793        let paths = planner.plan_independent(&agents).unwrap();
794
795        assert!(matches!(
796            first_conflict(&paths, 12).unwrap().kind,
797            StlCbsConflictKind::Vertex { .. } | StlCbsConflictKind::Edge { .. }
798        ));
799    }
800
801    #[test]
802    fn cbs_resolves_two_agent_swap() {
803        let planner = corridor_planner();
804        let agents = [
805            StlCbsAgent::new(0, (0, 1), (4, 1)),
806            StlCbsAgent::new(1, (4, 1), (0, 1)),
807        ];
808        let plan = planner.plan(&agents).unwrap();
809
810        assert!(plan.conflicts_resolved > 0);
811        assert!(first_conflict(&plan.paths, 12).is_none());
812        assert!(plan.min_pairwise_separation_robustness >= 0.0);
813    }
814
815    #[test]
816    fn stl_goal_and_avoid_robustness_have_expected_signs() {
817        let path = StlCbsPath {
818            agent_id: 0,
819            waypoints: vec![
820                StlTimedCell::new(0, 0, 0),
821                StlTimedCell::new(1, 0, 1),
822                StlTimedCell::new(2, 0, 2),
823            ],
824        };
825        let goal = StlRectangle2D::new(1.5, 2.5, -0.5, 0.5).unwrap();
826        let hazard = StlRectangle2D::new(0.8, 1.2, -0.2, 0.2).unwrap();
827        let interval = StlTimeInterval::new(0, 3).unwrap();
828
829        assert!(stl_eventually_reach_robustness(&path, goal, interval).unwrap() > 0.0);
830        assert!(stl_always_avoid_robustness(&path, hazard, interval).unwrap() < 0.0);
831    }
832}
rust_robotics_planning/stl_cbs.rs

rust_robotics_planning/
stl_cbs.rs
