public override void Handle(Ros2Message message)
{
var odometry = message.CastTo <OdometryMessage>();
if (odometry == null)
{
return;
}
var pos = new Vector3((float)odometry.pos_x, (float)odometry.pos_y, (float)odometry.pos_z);
var rot = new Quaternion((float)odometry.rot_x, (float)odometry.rot_y, (float)odometry.rot_z,
(float)odometry.rot_w);
RosMessageConvertExtender.Converter?.Convert(ref pos, ref rot);
SlamTrackedObject obj = new SlamTrackedObject(0, pos, rot);
if (_container.Count == 0)
{
_container.Add(obj);
}
else
{
_container.Update(obj);
}
message.Dispose();
}
public override void Handle(Ros2Message message)
{
var cloud = message.CastTo <PointCloud2Message>();
if (cloud == null)
{
return;
}
_container.Clear();
_container.AddRange(cloud.ToSlamPoints());
}
public static T?CastTo <T>(this Ros2Message from) where T : Ros2Message
{
MethodInfo?cPtrGetter = from.GetType().GetMethod("getCPtr", BindingFlags.NonPublic
| BindingFlags.Static);
return(cPtrGetter == null
? default
: (T)Activator.CreateInstance
(
typeof(T),
BindingFlags.NonPublic | BindingFlags.Instance,
null,
new object[] { ((HandleRef)cPtrGetter.Invoke(null, new object[] { from })).Handle, true },
null
));
}
public override void Handle(Ros2Message message)
{
var image = message.CastTo <ImageMessage>();
if (image is null)
{
return;
}
_presenter.Present(new ImageData
{
Width = (int)image.width,
Height = (int)image.height,
Encoding = ImageDataExt.GetTextureFormat(image.encoding),
Data = image.data.ToArray()
});
message.Dispose();
}
