public override bool SerializeMessage(Pipeline pipeline, Exporter store, string streamName, IEnumerable <RosMessage> messages, string messageType)
{
switch (messageType)
{
case "std_msgs/Int8":
case "std_msgs/UInt8":
case "std_msgs/Int16":
case "std_msgs/UInt16":
case "std_msgs/Int32":
case "std_msgs/UInt32":
case "std_msgs/Int64":
case "std_msgs/UInt64":
case "std_msgs/Float32":
case "std_msgs/Float64":
case "std_msgs/String":
case "std_msgs/Bool":
DynamicSerializers.WriteDynamic(pipeline, streamName, messages.Select(m => (m.GetField("data"), m.Time.ToDateTime() + this.Offset)), store);
return(true);
case "std_msgs/Header":
// For header, return time for now
DynamicSerializers.WriteDynamic(pipeline, streamName, messages.Select(m =>
{
var headerTime = m.GetField("stamp") as RosTime;
return((dynamic)headerTime.ToDateTime(), m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
default: return(false);
}
}
public bool SerializeMessage(Pipeline pipeline, Exporter store, string streamName, List <RosMessage> messages, string messageType)
{
switch (messageType)
{
case ("sensor_msgs/Image"):
DynamicSerializers.WriteDynamic(pipeline, streamName, messages.Select(m => (this.RosMessageToPsiImage(m), m.Time.ToDateTime())), store);
return(true);
default: return(false);
}
}
public bool SerializeMessage(Pipeline pipeline, Exporter store, string streamName, List <RosMessage> messages, string messageType)
{
switch (messageType)
{
case "std_msgs/Int8":
case "std_msgs/UInt8":
case "std_msgs/Int16":
case "std_msgs/UInt16":
case "std_msgs/Int32":
case "std_msgs/UInt32":
case "std_msgs/Int64":
case "std_msgs/UInt64":
case "std_msgs/Float32":
case "std_msgs/Float64":
case "std_msgs/String":
case "std_msgs/Bool":
DynamicSerializers.WriteDynamic(pipeline, streamName, messages.Select(m => (m.GetField("data"), m.Time.ToDateTime())), store);
return(true);
default: return(false);
}
}
public override bool SerializeMessage(Pipeline pipeline, Exporter store, string streamName, IEnumerable <RosMessage> messages, string messageType)
{
try
{
switch (messageType)
{
case ("sensor_msgs/Image"):
DynamicSerializers.WriteStronglyTyped <Shared <Image> >(pipeline, streamName, messages.Select(m => (this.ImageToPsiImage(m), m.Time.ToDateTime() + this.Offset)), store);
return(true);
case ("sensor_msgs/CompressedImage"):
// get header
DynamicSerializers.WriteStronglyTyped <Shared <Image> >(pipeline, streamName, messages.Select(m => (this.CompressedImageToPsiImage(m), this.useHeaderTime ? ((RosHeader)m.GetField("header")).Time.ToDateTime() + this.Offset : m.Time.ToDateTime() + this.Offset)), store);
return(true);
default: return(false);
}
}
catch (NotSupportedException)
{
// Not supported default to total copy
return(false);
}
}
public override bool SerializeMessage(Pipeline pipeline, Exporter store, string streamName, IEnumerable <RosMessage> messages, string messageType)
{
try
{
switch (messageType)
{
case ("geometry_msgs/Quaternion"):
DynamicSerializers.WriteStronglyTyped <Quaternion>(pipeline, streamName, messages.Select(m =>
{
return(new Quaternion(m.GetField("w"), m.GetField("x"), m.GetField("y"), m.GetField("z")), m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
case ("geometry_msgs/Point"):
DynamicSerializers.WriteStronglyTyped <Point3D>(pipeline, streamName, messages.Select(m =>
{
return(new Point3D(m.GetField("x"), m.GetField("y"), m.GetField("z")), m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
case ("geometry_msgs/PointStamped"):
DynamicSerializers.WriteStronglyTyped <Point3D>(pipeline, streamName, messages.Select(m =>
{
var pointObject = m.GetFieldAsRosMessage("point");
return(new Point3D(pointObject.GetField("x"), pointObject.GetField("y"), pointObject.GetField("z")), this.useHeaderTime ? ((RosHeader)m.GetField("header")).Time.ToDateTime() + this.Offset : m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
case ("geometry_msgs/Pose"):
DynamicSerializers.WriteStronglyTyped <CoordinateSystem>(pipeline, streamName, messages.Select(m =>
{
// get the orientation & Position
var ori = m.GetFieldAsRosMessage("orientation");
var quaternion = new Quaternion(ori.GetField("w"), ori.GetField("x"), ori.GetField("y"), ori.GetField("z"));
var pos = m.GetFieldAsRosMessage("position");
var point = new Point3D(pos.GetField("x"), pos.GetField("y"), pos.GetField("z"));
return(this.ConvertQuaternionToMatrix(quaternion, point), m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
case ("geometry_msgs/PoseStamped"):
DynamicSerializers.WriteStronglyTyped <CoordinateSystem>(pipeline, streamName, messages.Select(m =>
{
var p = m.GetField("Pose");
// get the orientation & Position
var ori = p.GetField("orientation");
var quaternion = new Quaternion(ori.GetField("w"), ori.GetField("x"), ori.GetField("y"), ori.GetField("z"));
var pos = p.GetField("position");
var point = new Point3D(pos.GetField("x"), pos.GetField("y"), pos.GetField("z"));
// TODO publish the header seperately
return(this.ConvertQuaternionToMatrix(quaternion, point), this.useHeaderTime ? ((RosHeader)m.GetField("header")).Time.ToDateTime() + this.Offset : m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
case ("geometry_msgs/Transform"):
DynamicSerializers.WriteStronglyTyped <CoordinateSystem>(pipeline, streamName, messages.Select(m =>
{
// get the orientation & Position
var ori = m.GetField("rotation");
var quaternion = new Quaternion(ori.GetField("w"), ori.GetField("x"), ori.GetField("y"), ori.GetField("z"));
var pos = m.GetField("translation");
var point = new Point3D(pos.GetField("x"), pos.GetField("y"), pos.GetField("z"));
return(this.ConvertQuaternionToMatrix(quaternion, point), m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
case ("geometry_msgs/Vector3"):
DynamicSerializers.WriteStronglyTyped <Vector3D>(pipeline, streamName, messages.Select(m =>
{
return(new Vector3D(m.GetField("x"), m.GetField("y"), m.GetField("z")), m.Time.ToDateTime() + this.Offset);
}), store);
return(true);
default: return(false);
}
}
catch (NotSupportedException)
{
// Not supported default to total copy
return(false);
}
}
