本文整理汇总了C++中EulerAngles::getPitch方法的典型用法代码示例。如果您正苦于以下问题：C++ EulerAngles::getPitch方法的具体用法？C++ EulerAngles::getPitch怎么用？C++ EulerAngles::getPitch使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类EulerAngles的用法示例。
在下文中一共展示了EulerAngles::getPitch方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: main
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");
}
}
}
}