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);
            }
        }
Пример #2
0
        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]);
            }
        }
Пример #3
0
        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();
            }
        }
Пример #4
0
 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);
     }
 }
Пример #5
0
 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);
     }
 }
Пример #6
0
        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);
        }
Пример #7
0
 public static int getCdrSerializedSize(sensor_msgs.msg.Imu data)
 {
     return(getCdrSerializedSize(data, 0));
 }
Пример #8
0
 public static void Copy(sensor_msgs.msg.Imu src, sensor_msgs.msg.Imu target)
 {
     target.Set(src);
 }