int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_tf_broadcaster");
ros::NodeHandle n;
ros::Rate loop_rate(1000); // Hz
Broadcaster broadcaster;
ros::Subscriber accelSub = n.subscribe("accel", 1000, &Broadcaster::accelCallback, &broadcaster);
ros::Subscriber magnetSub = n.subscribe("magnet", 1000, &Broadcaster::magnetCallback, &broadcaster);
ros::Subscriber headingSub = n.subscribe("heading", 1000, &Broadcaster::headingCallback, &broadcaster);
ros::Subscriber gyroSub = n.subscribe("gyro", 1000, &Broadcaster::gyroCallback, &broadcaster);
while(n.ok())
{
//broadcaster.updateTimers();
//broadcaster.updateRotation();
broadcaster.tfBroadcaster->sendTransform(
tf::StampedTransform(
tf::Transform(broadcaster.getQ(), tf::Vector3(0.0, 0.0, 0.0)),
ros::Time::now(), "odom", "imu_link") );
ros::spinOnce();
loop_rate.sleep();
// std::cout << "dt: " << broadcaster.getDt() << std::endl;
// std::cout << "q.x: " << broadcaster.getQ().x() << std::endl;
// std::cout << "q.y: " << broadcaster.getQ().y() << std::endl;
// std::cout << "q.z: " << broadcaster.getQ().z() << std::endl;
// std::cout << "q.w: " << broadcaster.getQ().w() << std::endl;
// std::cout << "----" << std::endl;
}
return 0;
}
