use rosrust::{MsgMessage, MsgValue, Time};
use std::convert::TryInto;
mod msg {
rosrust::rosmsg_include!(geometry_msgs / PoseArray);
}
fn make_typed_message() -> msg::geometry_msgs::PoseArray {
msg::geometry_msgs::PoseArray {
header: msg::std_msgs::Header {
seq: 22,
stamp: Time { sec: 123, nsec: 0 },
frame_id: "abc".into(),
},
poses: vec![
msg::geometry_msgs::Pose {
position: msg::geometry_msgs::Point {
x: 1.0,
y: 2.0,
z: 3.0,
},
orientation: msg::geometry_msgs::Quaternion {
x: 4.0,
y: 5.0,
z: 6.0,
w: 7.0,
},
},
msg::geometry_msgs::Pose {
position: msg::geometry_msgs::Point {
x: 8.0,
y: 9.0,
z: 10.0,
},
orientation: msg::geometry_msgs::Quaternion {
x: 11.0,
y: 12.0,
z: 13.0,
w: 14.0,
},
},
],
}
}
fn make_dynamic_message() -> MsgMessage {
let mut header = MsgMessage::new();
header.insert("seq".into(), MsgValue::U32(22));
header.insert("stamp".into(), MsgValue::Time(Time { sec: 123, nsec: 0 }));
header.insert("frame_id".into(), MsgValue::String("abc".into()));
let mut position1 = MsgMessage::new();
position1.insert("x".into(), MsgValue::F64(1.0));
position1.insert("y".into(), MsgValue::F64(2.0));
position1.insert("z".into(), MsgValue::F64(3.0));
let mut orientation1 = MsgMessage::new();
orientation1.insert("x".into(), MsgValue::F64(4.0));
orientation1.insert("y".into(), MsgValue::F64(5.0));
orientation1.insert("z".into(), MsgValue::F64(6.0));
orientation1.insert("w".into(), MsgValue::F64(7.0));
let mut pose1 = MsgMessage::new();
pose1.insert("position".into(), MsgValue::Message(position1));
pose1.insert("orientation".into(), MsgValue::Message(orientation1));
let mut position2 = MsgMessage::new();
position2.insert("x".into(), MsgValue::F64(8.0));
position2.insert("y".into(), MsgValue::F64(9.0));
position2.insert("z".into(), MsgValue::F64(10.0));
let mut orientation2 = MsgMessage::new();
orientation2.insert("x".into(), MsgValue::F64(11.0));
orientation2.insert("y".into(), MsgValue::F64(12.0));
orientation2.insert("z".into(), MsgValue::F64(13.0));
orientation2.insert("w".into(), MsgValue::F64(14.0));
let mut pose2 = MsgMessage::new();
pose2.insert("position".into(), MsgValue::Message(position2));
pose2.insert("orientation".into(), MsgValue::Message(orientation2));
let poses = vec![MsgValue::Message(pose1), MsgValue::Message(pose2)];
let mut message = MsgMessage::new();
message.insert("header".into(), MsgValue::Message(header));
message.insert("poses".into(), MsgValue::Array(poses));
message
}
#[test]
fn typed_message_to_dynamic_value() {
let typed_message = make_typed_message();
let dynamic_value: MsgValue = typed_message.into();
assert_eq!(dynamic_value, MsgValue::Message(make_dynamic_message()));
}
#[test]
fn typed_message_to_dynamic_message() {
let typed_message = make_typed_message();
let dynamic_message: MsgMessage = typed_message.into();
assert_eq!(dynamic_message, make_dynamic_message());
}
#[test]
fn dynamic_value_to_typed_message() {
let dynamic_value = MsgValue::Message(make_dynamic_message());
let typed_message: msg::geometry_msgs::PoseArray = dynamic_value
.try_into()
.expect("Failed to perform conversion");
assert_eq!(typed_message, make_typed_message());
}
#[test]
fn dynamic_message_to_typed_message() {
let dynamic_message = make_dynamic_message();
let typed_message: msg::geometry_msgs::PoseArray = dynamic_message
.try_into()
.expect("Failed to perform conversion");
assert_eq!(typed_message, make_typed_message());
}