import { Input, Message, Time } from "./types";
import { subtractTimes } from "./time";
type InputType = Message<"geometry_msgs/PoseStamped">;
type Output = {
header: InputType["header"];
time_diff: Time;
translation: Message<"geometry_msgs/Vector3">;
random_array: Array<number>;
};
export const inputs = ["/pose_stamped"];
export const output = "/pose_difference";
let lastMessage: InputType | undefined;
export default function script(
event: Input<"/pose_stamped">,
): Output | undefined {
const message = event.message;
if (!lastMessage) {
lastMessage = message;
return undefined;
}
const output: Output = {
header: message.header,
time_diff: subtractTimes(message.header.stamp, lastMessage.header.stamp),
translation: {
x: message.pose.position.x - lastMessage.pose.position.x,
y: message.pose.position.y - lastMessage.pose.position.y,
z: message.pose.position.z - lastMessage.pose.position.z,
},
random_array: [Math.random(), Math.random()],
};
lastMessage = message;
return output;
}