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); }