This is a pretty standard task for an IMU filter - like https://wiki.ros.org/imu_filter_madgw... . Check out the docs, but the setup should be pretty straight-forward.
Here's a sample how we launch it on one of our robots:
<node name="imu_filter" pkg="nodelet" type="nodelet" args="standalone imu_filter_madgwick/ImuFilterNodelet"
respawn="true">
<param name="use_mag" value="false" />
<param name="publish_tf" value="false" />
<param name="gain" value="0.01" />
<param name="constant_dt" value="0.01" />
<param name="orientation_stddev" value="0.1" />
<param name="time_jump_threshold" value="10.0" />
<remap from="imu/data_raw" to="imu_unbiased/data" />
<remap from="imu/data" to="imu_filtered/data" />
</node>
As you can see, we're subscribing to imu_unbiased/data, which is a topic that contains the raw IMU data with angular velocity bias removed (the unbiasing might be a crucial step for you to get steady performance without too large yaw drift). There is, unfortunately, no general-use package that could do the unbiasing for you. You would have to write it yourself or wait until I open-source our own package (no ETA though). One other thing you can try is using imu_complementary_filter instead of madgwick, which has an option for a builtin unbiasing. But I found the complementary filter a little worse than madgwick with our Xsens IMUs.
The general idea of unbiasing is to start the driver, instruct the user to not move the IMU for 5 seconds, accumulate data during this period, and then looking at the accumulated angular velocities. The bias removal node then alters the angular velocity measurements so that the long-term accumulated velocities should be zero when the IMU is stationary.
