コード例 #1
0
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");
      }
    }
  }
}