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 static TrackedObjPb ToProtobuf(this SlamTrackedObject o, ICSConverter converter)
=> new TrackedObjPb
{
Id = o.Id,
Position = o.Position.ToProtobuf(converter),
Orientation = o.Rotation.ToProtobuf(converter),
Color = o.Color,
Message = o.Message
};
private void AddWithHistory()
{
var messages = this.FindAllPreviousMessages()
.Select(m => MessageParser.Parse(m.Data, Topic.Type, true) !.GetPose() !.ToUnity())
.ToList();
if (messages.Count == 0)
{
return;
}
var obj = new SlamTrackedObject(0, messages.Last().Item1, messages.Last().Item2);
var history = new SlamLine[messages.Count - 1];
for (int i = 0; i < messages.Count - 1; i++)
{
history[i] = new SlamLine(new SlamPoint(i, messages[i].Item1, Color.black),
new SlamPoint(i + 1, messages[i + 1].Item1, Color.black),
i);
}
AddWithHistory(obj, history);
}
public TrackContainer(TrackedObjectsContainer parent, SlamTrackedObject trackedObject)
{
_parent = parent;
_trackedObject = trackedObject;
}
