int main(int argc, char** argv) { UInt32 baudRate = 115200; Int32 maxSize = 115200; UInt8 aBuf[maxSize]; SerialInterface sp("/dev/controller_Imu", baudRate); signal(SIGINT, catchSig); ros::init(argc, argv, "SubImuController"); ros::NodeHandle nh; ros::Publisher imuAttitudePub = nh.advertise<std_msgs::Float32MultiArray>("IMU_Attitude", 1000); ros::Publisher imuAccelPub = nh.advertise<std_msgs::Float32MultiArray>("IMU_Accel_Debug", 1000); ros::Publisher imuGyroPub = nh.advertise<std_msgs::Float32MultiArray>("IMU_Gyro_Debug", 1000); ros::Rate loop_rate(1); //TODO redo the whole reading and writing while (ros::ok() && m_running) { int size = waitForGoodHeader(sp, aBuf); if (size > 0) { UInt8 expectedSize = MipPacket::MIP_PACKET_HEADER_SIZE + MipPacket::MIP_PACKET_FOOTER_SIZE + aBuf[MipPacket::PAYLOAD_LENGTH_OFFSET]; //size of a AHRS //printf("Expecting packet of size: %d\n",expectedSize); while (size < expectedSize && ros::ok() && m_running) { int tmpSize = sp.recv(&aBuf[size],1); if (tmpSize > 0) { size += tmpSize; } else if (tmpSize < 0) { printf("Error\n"); } } if (size == expectedSize) { if (aBuf[MipPacket::DESCRIPTOR_OFFSET] == 0x80) { MipPacket packet; packet.deserialize(aBuf, size); for (MipFieldNode* pIt = packet.getIterator(); pIt != NULL; pIt = pIt->m_pNext) { if (pIt->m_pData->getFieldDescriptor() == DataField::DATA_FIELD_EULER_ANGLES_SET) { EulerAngles* pData = static_cast<EulerAngles*>(pIt->m_pData); std_msgs::Float32MultiArray rawMsg; rawMsg.data.push_back(pData->getYaw()*(180.0/M_PI)); rawMsg.data.push_back(pData->getPitch()*(180.0/M_PI)); rawMsg.data.push_back(pData->getRoll()*(180.0/M_PI)); imuAttitudePub.publish(rawMsg); } else if (pIt->m_pData->getFieldDescriptor() == DataField::DATA_FIELD_SCALED_ACCELEROMETER_VECTOR_SET) { ScaledAccelerometerVector* pData = static_cast<ScaledAccelerometerVector*>(pIt->m_pData); std_msgs::Float32MultiArray rawMsg; rawMsg.data.push_back(pData->getX()); rawMsg.data.push_back(pData->getY()); rawMsg.data.push_back(pData->getZ()); imuAccelPub.publish(rawMsg); } else if (pIt->m_pData->getFieldDescriptor() == DataField::DATA_FIELD_SCALED_GYRO_VECTOR_SET) { ScaledGyroVector* pData = static_cast<ScaledGyroVector*>(pIt->m_pData); std_msgs::Float32MultiArray rawMsg; rawMsg.data.push_back(pData->getX()); rawMsg.data.push_back(pData->getY()); rawMsg.data.push_back(pData->getZ()); imuGyroPub.publish(rawMsg); } } } } else { //printf("bad packet\n"); } } } }