void AsctecProc::imuCalcDataCallback(const asctec_msgs::IMUCalcDataConstPtr& imu_calcdata_msg)
{
// publish imu message
sensor_msgs::ImuPtr imu_msg;
imu_msg = boost::make_shared<sensor_msgs::Imu>();
createImuMsg (imu_calcdata_msg, imu_msg);
imu_publisher_.publish(imu_msg);
// publish unfiltered height message
mav_msgs::HeightPtr height_msg;
height_msg = boost::make_shared<mav_msgs::Height>();
createHeightMsg (imu_calcdata_msg, height_msg);
height_publisher_.publish(height_msg);
// publish filtered height message
mav_msgs::HeightPtr height_filtered_msg;
height_filtered_msg = boost::make_shared<mav_msgs::Height>();
createHeightFilteredMsg (imu_calcdata_msg, height_filtered_msg);
height_filtered_publisher_.publish(height_filtered_msg);
}
void AsctecProc::imuCalcDataCallback(const asctec_msgs::IMUCalcDataConstPtr& imuCalcDataMsg)
{
// publish imu message
sensor_msgs::Imu imuMsg;
createImuMsg (imuCalcDataMsg, imuMsg);
imuPublisher_.publish(imuMsg);
// publish unfiltered height message
asctec_msgs::Height heightMsg;
createHeightMsg (imuCalcDataMsg, heightMsg);
heightPublisher_.publish(heightMsg);
// publish filtered height message
asctec_msgs::Height heightFilteredMsg;
createHeightFilteredMsg (imuCalcDataMsg, heightFilteredMsg);
heightFilteredPublisher_.publish(heightFilteredMsg);
/*
printf("IMU: %f %f %f\n", imuCalcDataMsg->angle_roll * ASC_TO_ROS_ANGLE,
imuCalcDataMsg->angle_nick * ASC_TO_ROS_ANGLE,
imuCalcDataMsg->angle_yaw * ASC_TO_ROS_ANGLE);
*/
// publish tf for testing
btTransform t;
btQuaternion orientation;
// orientation.setRPY(imuCalcDataMsg->angle_roll * ASC_TO_ROS_ANGLE,
// imuCalcDataMsg->angle_nick * ASC_TO_ROS_ANGLE,
// imuCalcDataMsg->angle_yaw * ASC_TO_ROS_ANGLE);
orientation.setValue(imuMsg.orientation.x, imuMsg.orientation.y, imuMsg.orientation.z, imuMsg.orientation.w);
t.setRotation (orientation);
t.setOrigin (btVector3(0,0,0));
tf::StampedTransform worldToOdomTf (t, ros::Time::now(), "navigation", "imu_raw");
tfBroadcaster_.sendTransform (worldToOdomTf);
}
