537 lines
20 KiB
Rust
537 lines
20 KiB
Rust
//! The `mavlink.*` nodes in a generated program, against the vehicle twin
|
|
//! (`flowrs_connect::mavlink::peer`: a copter built from the same XML, speaking MAVLink 2 on
|
|
//! a TCP port as SITL does).
|
|
//!
|
|
//! One graph holds every node of the family on one link: `"mavlink.telemetry"` reads
|
|
//! `GLOBAL_POSITION_INT` (latitude and longitude in degrees, altitude in millimetres),
|
|
//! `"mavlink.heartbeat"` reads the vehicle's mode, `"mavlink.command"` sets GUIDED under an
|
|
//! approval naming its hash, and `"mavlink.setpoint"` sends a position given in ENU (ROS's
|
|
//! frame) under an approved envelope. The program is generated (`Graph::to_rust`), built
|
|
//! against flowrs-connect's `mavlink` runtime and run; the test reads what it printed and
|
|
//! what the vehicle received:
|
|
//!
|
|
//! * the telemetry is the twin's position, each value in its unit, exactly (a latitude of
|
|
//! -353632621 degE7 is -35.3632621 degrees);
|
|
//! * the command went once, under its approval, and the vehicle acknowledged it;
|
|
//! * the setpoint arrived as NED (north 5, east -2.5, down -10 from ENU east -2.5, north 5,
|
|
//! up 10), and only because the envelope admitted it: the same graph with no approval, and
|
|
//! with a position outside the envelope, sends nothing and says why;
|
|
//! * the heartbeat names the vehicle's mode (GUIDED, once the command has set it).
|
|
//!
|
|
//! A second graph carries a command from ROS to the vehicle: `"ros.subscribe"` reads a
|
|
//! `geometry_msgs/PointStamped` goal on `/drone/goal` (ENU, metres, REP 103) and wires its
|
|
//! point into `"mavlink.setpoint"`. The partitioner makes it one Linux program, the ROS 2 node
|
|
//! and the MAVLink link together; the test publishes the goal over DDS on loopback and
|
|
//! requires the vehicle to receive it as the NED target, under the approved envelope. Run
|
|
//! first with no time to open DDS (`FLOWROS_OPEN_TIMEOUT_MS=0`), the same program must exit
|
|
//! at once and say why.
|
|
//!
|
|
//! The programs build under `$TMPDIR/flowrs-mavlink-nodes*`, their target in
|
|
//! `$TMPDIR/flowrs-mavlink-nodes-target`, offline, against the workspace lockfile. Every
|
|
//! run has a wall-clock limit.
|
|
|
|
use std::io::Read;
|
|
use std::path::{Path, PathBuf};
|
|
use std::process::{Command, Stdio};
|
|
use std::time::{Duration, Instant};
|
|
|
|
use flowrs_connect::mavlink::gate::{ENVELOPE_KIND, KIND};
|
|
use flowrs_connect::mavlink::nodes;
|
|
use flowrs_connect::mavlink::peer::{Peer, PeerConfig};
|
|
use flowrs_core::graph::{Graph, NodeId, WireMode};
|
|
use flowrs_instruments::grammar::Match;
|
|
use flowrs_instruments::proposal::{Phase, PlannedCommand, Proposal, ProposalTable, Target};
|
|
|
|
fn add(g: &mut Graph, ty: &str, props: &[(&str, &str)]) -> NodeId {
|
|
let n = g.add_node(ty, 0.0, 0.0);
|
|
for (k, v) in props {
|
|
g.nodes
|
|
.get_mut(&n)
|
|
.unwrap()
|
|
.properties
|
|
.insert((*k).into(), (*v).into());
|
|
}
|
|
n
|
|
}
|
|
|
|
fn print(g: &mut Graph, from: NodeId, port: &str) {
|
|
let p = add(g, "PrintLn", &[]);
|
|
g.add_wire(from, port, p, "input", WireMode::Move).unwrap();
|
|
}
|
|
|
|
/// A proposal of `kind` with `lines`, approved by a person and claimed by the envelope, as
|
|
/// flowrs-server hands it to the program that executes it.
|
|
fn claimed(kind: &str, endpoint: &str, lines: &[String]) -> Proposal {
|
|
let p = Proposal::new(
|
|
kind,
|
|
"mavlink nodes test".into(),
|
|
"agent",
|
|
"now",
|
|
vec![Target {
|
|
model: "mavlink".into(),
|
|
endpoint: endpoint.into(),
|
|
mock: false,
|
|
}],
|
|
Vec::new(),
|
|
lines
|
|
.iter()
|
|
.map(|l| PlannedCommand {
|
|
target: 0,
|
|
line: l.clone(),
|
|
tier: flowrs_instruments::Tier::Energize,
|
|
matched: Match::Raw,
|
|
phase: Phase::Main,
|
|
query: false,
|
|
})
|
|
.collect(),
|
|
Vec::new(),
|
|
);
|
|
let mut t = ProposalTable::new();
|
|
let id = t.insert(p).unwrap().id.clone();
|
|
let hash = t.get(&id).unwrap().hash.clone();
|
|
t.decide(&id, &hash, true, "human", "now", None).unwrap();
|
|
t.claim(&id, &hash).unwrap();
|
|
t.get(&id).unwrap().clone()
|
|
}
|
|
|
|
/// A node of `ty` on `endpoint` (the link every mavlink node here shares), with `props`.
|
|
fn mav(g: &mut Graph, ty: &str, endpoint: &str, props: &[(&str, &str)]) -> NodeId {
|
|
let n = add(g, ty, props);
|
|
let p = &mut g.nodes.get_mut(&n).unwrap().properties;
|
|
p.insert("link".into(), endpoint.into());
|
|
p.insert("timeout_ms".into(), "8000".into());
|
|
n
|
|
}
|
|
|
|
/// The graph: every mavlink node on `endpoint`, the setpoint at ENU `position`. Ordered as
|
|
/// the program runs them: the command (GUIDED) first, then the setpoint, which the vehicle
|
|
/// takes only in GUIDED.
|
|
fn graph(endpoint: &str, position: &str, cmd_approval: &str, env_approval: &str) -> Graph {
|
|
let mut g = Graph::new();
|
|
let trigger = add(&mut g, "BoolLiteral", &[("value", "true")]);
|
|
let cmd = mav(
|
|
&mut g,
|
|
"mavlink.command",
|
|
endpoint,
|
|
&[
|
|
("system", "1"),
|
|
("command", "MAV_CMD_DO_SET_MODE"),
|
|
("param1", "1"),
|
|
("param2", "4"),
|
|
("approval", cmd_approval),
|
|
],
|
|
);
|
|
g.add_wire(trigger, "value", cmd, "trigger", WireMode::Copy)
|
|
.unwrap();
|
|
print(&mut g, cmd, "status");
|
|
let pos = add(
|
|
&mut g,
|
|
"Control",
|
|
&[("widget", "array"), ("value", position)],
|
|
);
|
|
let sp = mav(
|
|
&mut g,
|
|
"mavlink.setpoint",
|
|
endpoint,
|
|
&[
|
|
("system", "1"),
|
|
("frame", "enu"),
|
|
("approval", env_approval),
|
|
],
|
|
);
|
|
g.add_wire(pos, "value", sp, "position", WireMode::Move)
|
|
.unwrap();
|
|
print(&mut g, sp, "status");
|
|
let tel = mav(&mut g, "mavlink.telemetry", endpoint, &[("field3", "alt")]);
|
|
for p in ["value", "value2", "value3", "time"] {
|
|
print(&mut g, tel, p);
|
|
}
|
|
let hb = mav(&mut g, "mavlink.heartbeat", endpoint, &[]);
|
|
print(&mut g, hb, "mode");
|
|
g
|
|
}
|
|
|
|
/// The workspace checkout.
|
|
fn checkout() -> PathBuf {
|
|
PathBuf::from(env!("CARGO_MANIFEST_DIR"))
|
|
.join("../..")
|
|
.canonicalize()
|
|
.unwrap()
|
|
}
|
|
|
|
/// Build the crate `cargo_toml` + `src/main.rs` = `src` under `$TMPDIR/<dir>`, offline
|
|
/// against the workspace lockfile; its binary `bin`.
|
|
fn build_crate(dir: &str, cargo_toml: &str, src: &str, bin: &str) -> PathBuf {
|
|
let dir = std::env::temp_dir().join(dir);
|
|
std::fs::create_dir_all(dir.join("src")).unwrap();
|
|
std::fs::write(dir.join("src/main.rs"), src).unwrap();
|
|
std::fs::write(dir.join("Cargo.toml"), cargo_toml).unwrap();
|
|
std::fs::copy(checkout().join("Cargo.lock"), dir.join("Cargo.lock")).unwrap();
|
|
let target = std::env::temp_dir().join("flowrs-mavlink-nodes-target");
|
|
let out = Command::new(std::env::var("CARGO").unwrap_or_else(|_| "cargo".into()))
|
|
.args(["build", "--quiet", "--offline", "--manifest-path"])
|
|
.arg(dir.join("Cargo.toml"))
|
|
.env("CARGO_TARGET_DIR", &target)
|
|
.output()
|
|
.unwrap();
|
|
assert!(
|
|
out.status.success(),
|
|
"the generated program did not build:\n{}\n--- source ---\n{src}",
|
|
String::from_utf8_lossy(&out.stderr)
|
|
);
|
|
target.join("debug").join(bin)
|
|
}
|
|
|
|
/// Build the program `src` against flowrs-connect's `mavlink` runtime.
|
|
fn build(src: &str) -> PathBuf {
|
|
let co = checkout();
|
|
let toml = format!(
|
|
"[package]\nname = \"flowrs-mavlink-nodes\"\nversion = \"0.0.0\"\nedition = \"2024\"\n\
|
|
publish = false\n\n[workspace]\n\n[dependencies]\n\
|
|
flowrs-connect = {{ path = {:?}, features = [\"mavlink\"] }}\n",
|
|
co.join("crates/flowrs-connect").display().to_string(),
|
|
);
|
|
build_crate("flowrs-mavlink-nodes", &toml, src, "flowrs-mavlink-nodes")
|
|
}
|
|
|
|
/// Run `bin` for at most `limit`, returning its exit success, stdout and stderr.
|
|
fn run(bin: &Path, limit: Duration) -> (bool, String, String) {
|
|
run_with(bin, limit, &[], &mut || {})
|
|
}
|
|
|
|
/// [`run`] with `env`, calling `meanwhile` every 20 ms while it runs.
|
|
fn run_with(
|
|
bin: &Path,
|
|
limit: Duration,
|
|
env: &[(&str, String)],
|
|
meanwhile: &mut dyn FnMut(),
|
|
) -> (bool, String, String) {
|
|
let mut child = Command::new(bin)
|
|
.envs(env.iter().map(|(k, v)| (*k, v.as_str())))
|
|
.env("FLOWRS_MAVLINK_WINDOW_MS", "8000")
|
|
.stdout(Stdio::piped())
|
|
.stderr(Stdio::piped())
|
|
.spawn()
|
|
.unwrap();
|
|
let start = Instant::now();
|
|
let ok = loop {
|
|
if let Some(s) = child.try_wait().unwrap() {
|
|
break s.success();
|
|
}
|
|
if start.elapsed() > limit {
|
|
let _ = child.kill();
|
|
let _ = child.wait();
|
|
panic!("the generated program ran past {limit:?}");
|
|
}
|
|
meanwhile();
|
|
std::thread::sleep(Duration::from_millis(20));
|
|
};
|
|
let (mut o, mut e) = (String::new(), String::new());
|
|
child.stdout.take().unwrap().read_to_string(&mut o).unwrap();
|
|
child.stderr.take().unwrap().read_to_string(&mut e).unwrap();
|
|
(ok, o, e)
|
|
}
|
|
|
|
/// Build the generated program `src` and run it, for at most 90 s.
|
|
fn run_generated(src: &str) -> (bool, String, String) {
|
|
run(&build(src), Duration::from_secs(90))
|
|
}
|
|
|
|
#[test]
|
|
fn every_mavlink_node_runs_against_the_vehicle_twin() {
|
|
let d = nodes::dialect("ardupilotmega").unwrap();
|
|
let scratch = std::env::temp_dir().join(format!("flowrs-mavlink-nodes-{}", std::process::id()));
|
|
std::fs::create_dir_all(&scratch).unwrap();
|
|
let peer = Peer::spawn(PeerConfig::copter(d.clone())).unwrap();
|
|
let ep = peer.endpoint();
|
|
let count = |name: &str, command: Option<u64>| {
|
|
peer.log()
|
|
.received
|
|
.iter()
|
|
.filter(|g| {
|
|
g.name == name
|
|
&& command.is_none_or(|c| {
|
|
g.values.get("command").and_then(serde_json::Value::as_u64) == Some(c)
|
|
})
|
|
})
|
|
.count()
|
|
};
|
|
|
|
// Approvals, made on the twin's endpoint: GUIDED by its hash; the default envelope.
|
|
let guided = nodes::command_long(
|
|
&d,
|
|
(1, 1),
|
|
"MAV_CMD_DO_SET_MODE",
|
|
[1.0, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
|
)
|
|
.unwrap();
|
|
let cmd_file = scratch.join("cmd.json");
|
|
let line = format!("MAV_CMD_DO_SET_MODE GUIDED {}", guided.hash(&ep, "").tag());
|
|
std::fs::write(
|
|
&cmd_file,
|
|
serde_json::to_string(&claimed(KIND, &ep, &[line])).unwrap(),
|
|
)
|
|
.unwrap();
|
|
let env = nodes::envelope(&ep, (1, 1), 20.0, 30.0, 20.0, 300.0, "").unwrap();
|
|
let env_file = scratch.join("env.json");
|
|
std::fs::write(
|
|
&env_file,
|
|
serde_json::to_string(&claimed(ENVELOPE_KIND, &ep, &[env.describe()])).unwrap(),
|
|
)
|
|
.unwrap();
|
|
let (cmd_path, env_path) = (
|
|
cmd_file.display().to_string(),
|
|
env_file.display().to_string(),
|
|
);
|
|
|
|
// 1. Approved, inside the envelope: GUIDED, then the setpoint, then what the vehicle says.
|
|
let src = graph(&ep, "-2.5,5,10", &cmd_path, &env_path)
|
|
.to_rust()
|
|
.unwrap();
|
|
assert!(
|
|
src.contains("flowrs_connect::mavlink::graph::command("),
|
|
"{src}"
|
|
);
|
|
assert!(
|
|
src.contains("flowrs_connect::mavlink::graph::setpoint("),
|
|
"{src}"
|
|
);
|
|
assert!(
|
|
src.contains("flowrs_connect::mavlink::graph::telemetry("),
|
|
"{src}"
|
|
);
|
|
assert!(
|
|
src.contains("flowrs_connect::mavlink::graph::heartbeat("),
|
|
"{src}"
|
|
);
|
|
let (ok, out, err) = run_generated(&src);
|
|
assert!(ok, "the program failed:\n{out}\n{err}");
|
|
let lines: Vec<&str> = out.lines().collect();
|
|
assert_eq!(lines.len(), 7, "{out}\n{err}");
|
|
assert!(
|
|
lines[0].starts_with("MAV_CMD_DO_SET_MODE sent seq ") && lines[0].ends_with(": ACCEPTED"),
|
|
"{out}"
|
|
);
|
|
assert!(
|
|
lines[1].starts_with("sent seq ") && lines[1].ends_with(" north 5 east -2.5 down -10 m"),
|
|
"{out}"
|
|
);
|
|
// The twin's position, each value in its unit: degE7 to degrees, mm as mm.
|
|
assert_eq!(lines[2], "-35.3632621", "{out}");
|
|
assert_eq!(lines[3], "149.1652374", "{out}");
|
|
assert_eq!(lines[4], "584090", "{out}");
|
|
assert!(
|
|
lines[5].parse::<f64>().is_ok_and(|t| t >= 0.0),
|
|
"time_boot_ms: {out}"
|
|
);
|
|
// The newest heartbeat may predate the mode change; it is named either way.
|
|
assert!(matches!(lines[6], "STABILIZE" | "GUIDED"), "{out}");
|
|
let log = peer.log();
|
|
assert!(log.guided);
|
|
assert_eq!(log.target, Some([5.0, -2.5, -10.0]), "{log:?}");
|
|
assert_eq!(count("COMMAND_LONG", Some(176)), 1, "the command went once");
|
|
let setpoints = count("SET_POSITION_TARGET_LOCAL_NED", None);
|
|
assert_eq!(setpoints, 1, "{log:?}");
|
|
|
|
// 2. The same graph with no approval: the command and the setpoint send nothing and say
|
|
// why. The vehicle is still in GUIDED, and its heartbeat says so.
|
|
let src = graph(&ep, "-2.5,5,10", "", "").to_rust().unwrap();
|
|
let (ok, out, err) = run_generated(&src);
|
|
assert!(ok, "{out}\n{err}");
|
|
let lines: Vec<&str> = out.lines().collect();
|
|
assert_eq!(lines.len(), 7, "{out}\n{err}");
|
|
assert!(lines[0].starts_with("refused: no approval"), "{out}");
|
|
assert!(lines[1].starts_with("refused: no approval"), "{out}");
|
|
assert_eq!(lines[6], "GUIDED", "{out}");
|
|
assert_eq!(
|
|
count("COMMAND_LONG", Some(176)),
|
|
1,
|
|
"nothing unapproved went"
|
|
);
|
|
assert_eq!(count("SET_POSITION_TARGET_LOCAL_NED", None), setpoints);
|
|
|
|
// 3. Approved, but outside the envelope (up 31 m of 30): refused, unsent.
|
|
let src = graph(&ep, "0,0,31", "", &env_path).to_rust().unwrap();
|
|
let (ok, out, err) = run_generated(&src);
|
|
assert!(ok, "{out}\n{err}");
|
|
let lines: Vec<&str> = out.lines().collect();
|
|
assert!(
|
|
lines[1].starts_with("refused: down -31 m is outside the approved -30..0 m"),
|
|
"{out}"
|
|
);
|
|
assert_eq!(count("SET_POSITION_TARGET_LOCAL_NED", None), setpoints);
|
|
assert_eq!(peer.log().target, Some([5.0, -2.5, -10.0]));
|
|
let _ = std::fs::remove_dir_all(&scratch);
|
|
}
|
|
|
|
#[test]
|
|
fn a_ros_goal_reaches_the_vehicle_as_an_approved_setpoint() {
|
|
use flowrs_connect::ros2::EdgeClass;
|
|
use flowrs_connect::ros2::node::{Distro, EndpointSpec, NodeSpec};
|
|
use flowrs_connect::ros2::rt::RosNode;
|
|
use flowrs_core::codegen::partition::{Options, partition};
|
|
|
|
let d = nodes::dialect("ardupilotmega").unwrap();
|
|
let peer = Peer::spawn(PeerConfig::copter(d)).unwrap();
|
|
let ep = peer.endpoint();
|
|
let scratch = std::env::temp_dir().join(format!("flowrs-mavlink-goal-{}", std::process::id()));
|
|
std::fs::create_dir_all(&scratch).unwrap();
|
|
let env = nodes::envelope(&ep, (1, 1), 20.0, 30.0, 20.0, 300.0, "").unwrap();
|
|
let env_file = scratch.join("env.json");
|
|
std::fs::write(
|
|
&env_file,
|
|
serde_json::to_string(&claimed(ENVELOPE_KIND, &ep, &[env.describe()])).unwrap(),
|
|
)
|
|
.unwrap();
|
|
|
|
// ros.subscribe (a PointStamped goal, ENU) -> mavlink.setpoint (frame enu) -> its status.
|
|
let mut g = Graph::new();
|
|
let goal = add(
|
|
&mut g,
|
|
"ros.subscribe",
|
|
&[
|
|
("topic", "goal"),
|
|
("type", "geometry_msgs/msg/PointStamped"),
|
|
("field", "point"),
|
|
("domain", "ros(/drone)"),
|
|
],
|
|
);
|
|
let env_path = env_file.display().to_string();
|
|
let sp = mav(
|
|
&mut g,
|
|
"mavlink.setpoint",
|
|
&ep,
|
|
&[("system", "1"), ("frame", "enu"), ("approval", &env_path)],
|
|
);
|
|
g.add_wire(goal, "value", sp, "position", WireMode::Move)
|
|
.unwrap();
|
|
print(&mut g, sp, "status");
|
|
let co = checkout().display().to_string();
|
|
let p = partition(
|
|
&g,
|
|
&Options {
|
|
name: "flowrs_goal".into(),
|
|
host_passes: 1,
|
|
timeout_ms: 30_000,
|
|
checkout: co,
|
|
distro: Distro::Jazzy,
|
|
},
|
|
)
|
|
.unwrap_or_else(|e| panic!("{e:?}"));
|
|
let linux = p.linux().unwrap();
|
|
assert!(
|
|
linux.cargo_toml.contains("\"ros2\", \"mavlink\""),
|
|
"{}",
|
|
linux.cargo_toml
|
|
);
|
|
let calls = flowrs_connect::ros2::xcdr_gen::dynamic_codec_calls(&linux.source);
|
|
assert!(calls.is_empty(), "{calls:?}");
|
|
let bin = build_crate(
|
|
"flowrs-mavlink-goal",
|
|
&linux.cargo_toml,
|
|
&linux.source,
|
|
&linux.crate_name.replace('_', "-"),
|
|
);
|
|
|
|
let domain = 150 + (std::process::id() % 50) as u16;
|
|
// A program whose ROS 2 node cannot open DDS in time exits at once with the reason, not
|
|
// waiting for ever with nothing said (a stuck airportd did that on the build host,
|
|
// 2026-10-30). No participant opens in no time at all.
|
|
let start = Instant::now();
|
|
let (ok, out, err) = run_with(
|
|
&bin,
|
|
Duration::from_secs(60),
|
|
&[
|
|
("ROS_DOMAIN_ID", domain.to_string()),
|
|
("ROS_AUTOMATIC_DISCOVERY_RANGE", "LOCALHOST".into()),
|
|
("FLOWROS_OPEN_TIMEOUT_MS", "0".into()),
|
|
],
|
|
&mut || {},
|
|
);
|
|
assert!(
|
|
!ok,
|
|
"the program ran with no time to open DDS:\n{out}\n{err}"
|
|
);
|
|
// main returns the error, and Rust prints it: `Error: "DDS: opening the node /drone/…`.
|
|
assert!(
|
|
err.contains("Error: \"DDS: opening the node /drone/"),
|
|
"{out}\n{err}"
|
|
);
|
|
assert!(
|
|
err.contains(&format!(
|
|
"on ROS_DOMAIN_ID {domain} did not finish within 0 ms; the thread still waiting on \
|
|
it cannot be stopped, so the program must exit"
|
|
)),
|
|
"{out}\n{err}"
|
|
);
|
|
assert!(
|
|
start.elapsed() < Duration::from_secs(30),
|
|
"{:?}",
|
|
start.elapsed()
|
|
);
|
|
|
|
// The goal, published until the program has read one and exited.
|
|
let mut publisher = RosNode::open_with(
|
|
&NodeSpec {
|
|
name: "goal_source".into(),
|
|
namespace: "/drone".into(),
|
|
distro: Distro::Jazzy,
|
|
endpoints: vec![EndpointSpec {
|
|
name: "GOAL".into(),
|
|
node: 0,
|
|
topic: "/drone/goal".into(),
|
|
type_name: "geometry_msgs/msg/PointStamped".into(),
|
|
rihs01: flowrs_iface::ros::catalog::rihs01("geometry_msgs/msg/PointStamped")
|
|
.unwrap(),
|
|
publish: true,
|
|
class: EdgeClass::auto("geometry_msgs/msg/PointStamped"),
|
|
qos: EdgeClass::auto("geometry_msgs/msg/PointStamped").qos(),
|
|
paths: vec!["point".into()],
|
|
constants: String::new(),
|
|
frame_id: "map".into(),
|
|
field: String::new(),
|
|
device_clock: String::new(),
|
|
}],
|
|
},
|
|
domain,
|
|
true,
|
|
)
|
|
.unwrap();
|
|
let mut last = Instant::now() - Duration::from_secs(1);
|
|
let mut sent = 0u32;
|
|
let (ok, out, err) = run_with(
|
|
&bin,
|
|
Duration::from_secs(90),
|
|
&[
|
|
("ROS_DOMAIN_ID", domain.to_string()),
|
|
("ROS_AUTOMATIC_DISCOVERY_RANGE", "LOCALHOST".into()),
|
|
("FLOWROS_WAIT_MATCHED_MS", "30000".into()),
|
|
],
|
|
&mut || {
|
|
if last.elapsed() >= Duration::from_millis(100) {
|
|
let m = publisher
|
|
.compose(0, &[serde_json::json!([-2.5, 5.0, 10.0])])
|
|
.unwrap();
|
|
publisher.publish(0, m, None).unwrap();
|
|
sent += 1;
|
|
last = Instant::now();
|
|
}
|
|
},
|
|
);
|
|
assert!(ok, "the program failed after {sent} goals:\n{out}\n{err}");
|
|
assert!(
|
|
out.lines()
|
|
.any(|l| l.starts_with("sent seq ") && l.ends_with(" north 5 east -2.5 down -10 m")),
|
|
"{out}\n{err}"
|
|
);
|
|
let got: Vec<_> = peer
|
|
.log()
|
|
.received
|
|
.into_iter()
|
|
.filter(|g| g.name == "SET_POSITION_TARGET_LOCAL_NED")
|
|
.collect();
|
|
assert_eq!(got.len(), 1, "{got:?}");
|
|
let v = |k: &str| got[0].values[k].as_f64().unwrap();
|
|
assert_eq!((v("x"), v("y"), v("z")), (5.0, -2.5, -10.0));
|
|
let _ = std::fs::remove_dir_all(&scratch);
|
|
}
|