public static void NewFrameReceived(object sender, EventArgs e)
{
float[] tempFloatData = new float[9];
UInt16[] tempIntData = new UInt16[9];
int numFrame = device.getNumFrames() - 1;
tempIntData = sensor.getDigitalFrame(numFrame).toArray();
for (int j = 0; j < numData; ++j)
{
intDataArray1[j, dataLength - 1] = tempIntData[j];
Array.Copy(intDataArray1, dataLength * j + 1, intDataArray1, dataLength * j, dataLength - 1);
}
for (int col = 0; col < dataLength; col++)
{
sensor_msgs.msg.Imu imu_data_msg = new sensor_msgs.msg.Imu();
imu_data_msg.Linear_acceleration.X = intDataArray1[col, 0];
imu_data_msg.Linear_acceleration.Y = intDataArray1[col, 1];
imu_data_msg.Linear_acceleration.Z = intDataArray1[col, 2];
imu_data_msg.Angular_velocity.X = intDataArray1[col, 3];
imu_data_msg.Angular_velocity.Y = intDataArray1[col, 4];
imu_data_msg.Angular_velocity.Z = intDataArray1[col, 5];
Quaternion q = Quaternion.CreateFromYawPitchRoll(intDataArray1[col, 6], intDataArray1[col, 7], intDataArray1[col, 8]);
imu_data_msg.Orientation.X = q.X;
imu_data_msg.Orientation.Y = q.Y;
imu_data_msg.Orientation.Z = q.Z;
imu_data_msg.Orientation.W = q.W;
imu_data_pub.Publish(imu_data_msg);
}
}
public static void write(sensor_msgs.msg.Imu data, Halodi.CDR.CDRSerializer cdr)
{
std_msgs.msg.HeaderPubSubType.write(data.header, cdr);
geometry_msgs.msg.QuaternionPubSubType.write(data.orientation, cdr);
for (int i0 = 0; i0 < 9; ++i0)
{
cdr.write_type_6(data.orientation_covariance[i0]);
}
geometry_msgs.msg.Vector3PubSubType.write(data.angular_velocity, cdr);
for (int i0 = 0; i0 < 9; ++i0)
{
cdr.write_type_6(data.angular_velocity_covariance[i0]);
}
geometry_msgs.msg.Vector3PubSubType.write(data.linear_acceleration, cdr);
for (int i0 = 0; i0 < 9; ++i0)
{
cdr.write_type_6(data.linear_acceleration_covariance[i0]);
}
}
public static void read(sensor_msgs.msg.Imu data, Halodi.CDR.CDRDeserializer cdr)
{
data.header = std_msgs.msg.HeaderPubSubType.Create();
std_msgs.msg.HeaderPubSubType.read(data.header, cdr);
data.orientation = geometry_msgs.msg.QuaternionPubSubType.Create();
geometry_msgs.msg.QuaternionPubSubType.read(data.orientation, cdr);
for (int i0 = 0; i0 < 9; ++i0)
{
data.orientation_covariance[i0] = cdr.read_type_6();
}
data.angular_velocity = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(data.angular_velocity, cdr);
for (int i0 = 0; i0 < 9; ++i0)
{
data.angular_velocity_covariance[i0] = cdr.read_type_6();
}
data.linear_acceleration = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(data.linear_acceleration, cdr);
for (int i0 = 0; i0 < 9; ++i0)
{
data.linear_acceleration_covariance[i0] = cdr.read_type_6();
}
}
public override void deserialize(MemoryStream stream, sensor_msgs.msg.Imu data)
{
using (BinaryReader reader = new BinaryReader(stream))
{
Halodi.CDR.CDRDeserializer cdr = new Halodi.CDR.CDRDeserializer(reader);
read(data, cdr);
}
}
public override void serialize(sensor_msgs.msg.Imu data, MemoryStream stream)
{
using (BinaryWriter writer = new BinaryWriter(stream))
{
Halodi.CDR.CDRSerializer cdr = new Halodi.CDR.CDRSerializer(writer);
write(data, cdr);
}
}
public static int getCdrSerializedSize(sensor_msgs.msg.Imu data, int current_alignment)
{
int initial_alignment = current_alignment;
current_alignment += std_msgs.msg.HeaderPubSubType.getCdrSerializedSize(data.header, current_alignment);
current_alignment += geometry_msgs.msg.QuaternionPubSubType.getCdrSerializedSize(data.orientation, current_alignment);
current_alignment += ((9) * 8) + Halodi.CDR.CDRCommon.alignment(current_alignment, 8);
current_alignment += geometry_msgs.msg.Vector3PubSubType.getCdrSerializedSize(data.angular_velocity, current_alignment);
current_alignment += ((9) * 8) + Halodi.CDR.CDRCommon.alignment(current_alignment, 8);
current_alignment += geometry_msgs.msg.Vector3PubSubType.getCdrSerializedSize(data.linear_acceleration, current_alignment);
current_alignment += ((9) * 8) + Halodi.CDR.CDRCommon.alignment(current_alignment, 8);
return(current_alignment - initial_alignment);
}
public static int getCdrSerializedSize(sensor_msgs.msg.Imu data)
{
return(getCdrSerializedSize(data, 0));
}
public static void Copy(sensor_msgs.msg.Imu src, sensor_msgs.msg.Imu target)
{
target.Set(src);
}
