diff --git a/config/simulations/Junction Twoway/config.toml b/config/simulations/Junction Twoway/config.toml index 5ef2abd..2efb1cb 100644 --- a/config/simulations/Junction Twoway/config.toml +++ b/config/simulations/Junction Twoway/config.toml @@ -1,117 +1,116 @@ -# environment = "junction" environment_image = "junction_twoway" -environment = "./config/simulations/Intersection/environment.yaml" -formation_group = "./config/simulations/Intersection/formation.ron" - -[interaction] -ui-focus-cancels-inputs = true -default-cam-distance = 100.0 - -[visualisation.uncertainty] -max-radius = 2.5 -scale = 30.0 +environment = "./config/simulations/Intersection/environment.yaml" +formation_group = "./config/simulations/Intersection/formation.ron" [visualisation.height] -objects = 0.5 +objects = 0.5 height-map = 1.0 [visualisation.draw] -robots = true -communication-graph = false -predicted-trajectories = false -waypoints = true -uncertainty = false -paths = true -generated-map = true -height-map = false -sdf = true -communication-radius = false -obstacle-factors = false -tracking = false -interrobot-factors = false +robots = true +communication-graph = true +predicted-trajectories = true +waypoints = true +uncertainty = false +paths = false +communication-radius = false +obstacle-factors = false +tracking = false +interrobot-factors = true interrobot-factors-safety-distance = false -robot-colliders = false -environment-colliders = false -robot-robot-collisions = false -robot-environment-collisions = false +generated-map = true +sdf = false +robot-colliders = false +environment-colliders = false +robot-robot-collisions = false +robot-environment-collisions = false +[visualisation.uncertainty] +max-radius = 2.5 +scale = 30.0 -[gbp] -sigma-pose-fixed = 1e-15 -sigma-factor-dynamics = 0.1 -sigma-factor-interrobot = 0.001 -sigma-factor-obstacle = 0.001 -sigma-factor-tracking = 0.01 -lookahead-multiple = 3 +[interaction] +ui-focus-cancels-inputs = true +default-cam-distance = 100.0 -[gbp.factors-enabled] -tracking = false -obstacle = true +[gbp] +sigma-pose-fixed = 0.0000000000000010000000036274937 +sigma-factor-dynamics = 0.10000000149011612 +sigma-factor-interrobot = 0.009999999776482582 +sigma-factor-obstacle = 0.009999999776482582 +sigma-factor-tracking = 0.009999999776482582 +lookahead-multiple = 3 +variables = 10 + +[gbp.tracking] +switch-padding = 1.0 +attraction-distance = 2.0 [gbp.iteration-schedule] internal = 10 external = 10 schedule = "interleave-evenly" +[gbp.factors-enabled] +dynamic = true +interrobot = true +obstacle = true +tracking = false + [robot] -planning-horizon = 2.0 -target-speed = 15.0 -inter-robot-safety-distance-multiplier = 2.2 +planning-horizon = 5.0 +target-speed = 5.0 +inter-robot-safety-distance-multiplier = 2.5 [robot.radius] min = 1.0 max = 1.0 [robot.communication] -radius = 20.0 +radius = 20.0 failure-rate = 0.0 [simulation] -max-time = 10000.0 -time-scale = 1.0 +max-time = 10000.0 +time-scale = 2.0 manual-step-factor = 1 -hz = 10.0 -# world-size = 100.0 -prng-seed = 2 -pause-on-spawn = false -despawn-robot-when-final-waypoint-reached = false - +hz = 10.0 +prng-seed = 2 +pause-on-spawn = false +despawn-robot-when-final-waypoint-reached = true +exit-application-on-scenario-finished = false [rrt] -max-iterations = 5000000 -step-size = 5.0 -collision-radius = 3.0 +max-iterations = 5000000 +step-size = 5.0 +collision-radius = 3.0 neighbourhood-radius = 8.0 [rrt.smoothing] -enabled = true +enabled = true max-iterations = 500 -step-size = 0.5 - -# [rrt] -# max-iterations = 10000000 -# step-size = 0.5 -# collision-radius = 0.1 -# neighbourhood-radius = 10.0 -# -# [rrt.smoothing] -# enabled = false -# max-iterations = 100 -# step-size = 1.0 +step-size = 0.5 [graphviz] export-location = "./assets/export/" [graphviz.interrobot.active] style = "dashed" -len = 8.0 +len = 8.0 color = "red" [graphviz.interrobot.inactive] style = "dashed" -len = 8.0 +len = 8.0 color = "gray" - [manual] timesteps-per-step = 1 + +[debug.on-variable-clicked] +obstacle = false +dynamic = false +interrobot = false +tracking = false +variable = false +inbox = false diff --git a/config/simulations/Junction Twoway/environment.yaml b/config/simulations/Junction Twoway/environment.yaml index 23344eb..b4428b3 100644 --- a/config/simulations/Junction Twoway/environment.yaml +++ b/config/simulations/Junction Twoway/environment.yaml @@ -7,6 +7,6 @@ tiles: obstacle-height: 2.0 sdf: resolution: 200 - expansion: 0.05 + expansion: 0.01 blur: 0.01 obstacles: [] diff --git a/config/simulations/Junction Twoway/formation.yaml b/config/simulations/Junction Twoway/formation.yaml index 40fee53..78de31c 100644 --- a/config/simulations/Junction Twoway/formation.yaml +++ b/config/simulations/Junction Twoway/formation.yaml @@ -1,76 +1,160 @@ formations: -# - repeat: -# every: -# secs: 6 -# nanos: 0 -# times: !finite 10 -# delay: -# secs: 0 -# nanos: 0 -# robots: 1 -# planning-strategy: only-local -# initial-position: -# shape: !line-segment -# - x: -0.05 -# y: 0.55 -# - x: -0.05 -# y: 0.55 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 1.3 -# y: 0.55 -# - x: 1.3 -# y: 0.55 -# projection-strategy: identity -# waypoint-reached-when-intersects: current - - + # start left: turn left (red) - repeat: every: - secs: 3 + secs: 5 nanos: 0 - times: !finite 1 + times: !infinite delay: secs: 0 nanos: 0 robots: 1 - planning-strategy: rrt-star + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: -0.0 + y: 0.565 + - x: -0.0 + y: 0.515 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 0.45 + y: 0.565 + - x: 0.50 + y: 0.515 + projection-strategy: identity + - shape: !line-segment + - y: 1.15 + x: 0.45 + - y: 1.15 + x: 0.50 + projection-strategy: identity + waypoint-reached-when-intersects: + distance: robot-radius + intersects-with: horizon + finished-when-intersects: + # distance: robot-radius + # intersects-with: horizon + distance: !meter 10 + intersects-with: !variable 5 + # start left: turn right (blue) +- repeat: + every: + secs: 4 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 500000000 + robots: 1 + planning-strategy: only-local initial-position: shape: !line-segment - x: -0.0 - y: 0.45 + y: 0.515 - x: -0.0 - y: 0.45 + y: 0.565 placement-strategy: !random - attempts: 1000 + attempts: 2000 waypoints: - shape: !line-segment - - x: 0.525 - y: 0.45 - - x: 0.525 - y: 0.45 + - x: 0.515 + y: 0.515 + - x: 0.565 + y: 0.565 projection-strategy: identity - shape: !line-segment - - x: 0.525 - y: 1 - - x: 0.525 - y: 1 + - y: -0.15 + x: 0.515 + - y: -0.15 + x: 0.565 projection-strategy: identity - # waypoint-reached-when-intersects: current waypoint-reached-when-intersects: distance: robot-radius intersects-with: horizon finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + # start left: drive across (green) +- repeat: + every: + secs: 7 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 750000000 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: -0.0 + y: 0.565 + - x: -0.0 + y: 0.515 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 1.15 + y: 0.565 + - x: 1.15 + y: 0.515 + projection-strategy: identity + waypoint-reached-when-intersects: distance: robot-radius - intersects-with: current + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + # start top: turn left (red) - repeat: every: secs: 5 nanos: 0 - times: !finite 5 + times: !infinite + delay: + secs: 0 + nanos: 0 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: 0.515 + y: 1.0 + - x: 0.565 + y: 1.0 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 0.515 + y: 0.515 + - x: 0.565 + y: 0.565 + projection-strategy: identity + - shape: !line-segment + - x: 1.15 + y: 0.515 + - x: 1.15 + y: 0.565 + projection-strategy: identity + waypoint-reached-when-intersects: + distance: robot-radius + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + + # start top: turn right (blue) +- repeat: + every: + secs: 4 + nanos: 0 + times: !infinite delay: secs: 0 nanos: 500000000 @@ -78,83 +162,249 @@ formations: planning-strategy: only-local initial-position: shape: !line-segment - - x: -0.0 - y: 0.45 - - x: -0.0 - y: 0.45 + - x: 0.515 + y: 1.0 + - x: 0.565 + y: 1.0 placement-strategy: !random - attempts: 1000 + attempts: 2000 waypoints: - shape: !line-segment - - x: 0.525 - y: 0.0 - - x: 0.475 - y: 0.45 + - x: 0.515 + y: 0.475 + - x: 0.565 + y: 0.425 projection-strategy: identity - shape: !line-segment + - x: -0.15 + y: 0.475 + - x: -0.15 + y: 0.425 + projection-strategy: identity + waypoint-reached-when-intersects: + distance: robot-radius + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + + # start top: drive across (green) +- repeat: + every: + secs: 7 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 750000000 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: 0.515 + y: 1.0 + - x: 0.565 + y: 1.0 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 0.515 + y: -0.15 + - x: 0.565 + y: -0.15 + projection-strategy: identity + waypoint-reached-when-intersects: + distance: robot-radius + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + + + # start right: turn left (red) +- repeat: + every: + secs: 5 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 0 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment - x: 1.0 - y: 0.45 + y: 0.435 - x: 1.0 - y: 0.45 + y: 0.485 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 0.565 + y: 0.435 + - x: 0.515 + y: 0.485 + projection-strategy: identity + - shape: !line-segment + - x: 0.565 + y: -0.15 + - x: 0.515 + y: -0.15 + projection-strategy: identity + waypoint-reached-when-intersects: + distance: robot-radius + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + + + # start right: drive across (green) +- repeat: + every: + secs: 7 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 450000000 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: 1.0 + y: 0.435 + - x: 1.0 + y: 0.485 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: -0.15 + y: 0.435 + - x: -0.15 + y: 0.485 + projection-strategy: identity + waypoint-reached-when-intersects: + distance: robot-radius + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + + + # start bottom: turn left (red) +- repeat: + every: + secs: 5 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 500000000 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: 0.435 + y: 0.0 + - x: 0.485 + y: 0.0 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 0.435 + y: 0.435 + - x: 0.485 + y: 0.485 + projection-strategy: identity + - shape: !line-segment + - x: -0.15 + y: 0.435 + - x: -0.15 + y: 0.485 projection-strategy: identity - # waypoint-reached-when-intersects: current waypoint-reached-when-intersects: distance: robot-radius intersects-with: horizon finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 + + + # start bottom: drive across (green) +- repeat: + every: + secs: 7 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 0 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: 0.435 + y: 0.0 + - x: 0.485 + y: 0.0 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 0.435 + y: 1.15 + - x: 0.485 + y: 1.15 + projection-strategy: identity + waypoint-reached-when-intersects: distance: robot-radius - intersects-with: current + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 -# - repeat: -# every: -# secs: 3 -# nanos: 0 -# times: !finite 10 -# delay: -# secs: 0 -# nanos: 500000000 -# robots: 1 -# planning-strategy: only-local -# initial-position: -# shape: !line-segment -# - x: 0.45 -# y: 1.05 -# - x: 0.45 -# y: 1.05 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 0.45 -# y: -0.3 -# - x: 0.45 -# y: -0.3 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon -# - repeat: -# every: -# secs: 4 -# nanos: 0 -# times: !finite 10 -# delay: -# secs: 0 -# nanos: 0 -# robots: 1 -# planning-strategy: only-local -# initial-position: -# shape: !line-segment -# - x: 0.55 -# y: 1.05 -# - x: 0.55 -# y: 1.05 -# placement-strategy: !random -# attempts: 1000 -# waypoints: -# - shape: !line-segment -# - x: 0.55 -# y: -0.3 -# - x: 0.55 -# y: -0.3 -# projection-strategy: identity -# waypoint-reached-when-intersects: horizon + # start bottom: turn right (blue) +- repeat: + every: + secs: 6 + nanos: 0 + times: !infinite + delay: + secs: 0 + nanos: 800000000 + robots: 1 + planning-strategy: only-local + initial-position: + shape: !line-segment + - x: 0.435 + y: 0.0 + - x: 0.485 + y: 0.0 + placement-strategy: !random + attempts: 2000 + waypoints: + - shape: !line-segment + - x: 0.435 + y: 0.565 + - x: 0.485 + y: 0.515 + projection-strategy: identity + - shape: !line-segment + - x: 1.15 + y: 0.565 + - x: 1.15 + y: 0.515 + projection-strategy: identity + waypoint-reached-when-intersects: + distance: robot-radius + intersects-with: horizon + finished-when-intersects: + distance: !meter 10 + intersects-with: !variable 5 diff --git a/crates/magics/src/planner/spawner.rs b/crates/magics/src/planner/spawner.rs index 621cc0c..88f2376 100644 --- a/crates/magics/src/planner/spawner.rs +++ b/crates/magics/src/planner/spawner.rs @@ -502,18 +502,6 @@ fn spawn_formation( let Some(min_radius) = radii.iter().copied().map(ordered_float::OrderedFloat).min() else { return; }; - - // let min_radius = radii - // .iter() - // .copied() - // .map(ordered_float::OrderedFloat) - // .min() - // .expect("not empty"); - - // #[rustfmt::skip] - // let max_radius = - // radii.iter().copied().map(ordered_float::OrderedFloat).max().expect("not - // empty"); #[rustfmt::skip] let Some(max_radius) = radii.iter().copied().map(ordered_float::OrderedFloat).max() else { return;