robot_localization ignoring IMU frame orientation
Hello, I'm trying to implement robot_localization in my robot, with IMU sensor which is mounted upside-down. I tried to change orientation of imu_frame, but robot_localization seems to ignore the changes. More precisely: it ignores orientation of imu_frame when processing angular velocity (works correctly when processing orientation quaternion ~ when second line of imu0_config is "true, true, true" and fourth line is "false, false, false", but it does not fit my needs because of poor precision).
Details of my implementation:
robot_localization parameters are
odom_frame: odom
base_link_frame: base_footprint
world_frame: odom
two_d_mode: false
frequency: 50
odom0: husky_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, false, false,
false, false, false,
false, false, false]
odom0_differential: false
odom0_queue_size: 10
imu0: imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
false, false, true,
true, false, false]
imu0_differential: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
And I played the same bag file with topics needed by robot_localization three times, each time using different orientation of imu_frame.
case 1:
$ rosrun tf tf_echo base_footprint imu_link
At time 1537803279.035
- Translation: [-0.290, 0.000, 0.568]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY (radian) [0.000, -0.000, 0.000]
in RPY (degree) [0.000, -0.000, 0.000]
case 2:
$ rosrun tf tf_echo base_footprint imu_link
At time 1537803271.316
- Translation: [-0.290, 0.000, 0.568]
- Rotation: in Quaternion [1.000, 0.000, 0.000, -0.000]
in RPY (radian) [-3.142, -0.000, 0.000]
in RPY (degree) [-180.000, -0.000, 0.000]
case 3:
$ rosrun tf tf_echo base_footprint imu_link
At time 1537803264.615
- Translation: [-0.290, 0.000, 0.568]
- Rotation: in Quaternion [0.707, 0.000, 0.000, 0.707]
in RPY (radian) [1.570, -0.000, 0.000]
in RPY (degree) [89.954, -0.000, 0.000]
I expected that this experiment would result in three different trajectories of robot (in case 1 and case 2 robot should rotate in opposite directions and in case 3 there should have been no significant rotation at all, since the robot didn't rotate around other axes than z). But the reality is that the trajectories are exactly the same. For better explanation I append images of final robot position for each of the cases.
So is there a software solution for my problem? Is there parameter to enable rotated IMUs or is it a bug (or a reasoned limitation) of robot_localization package? Or do I need to remount my IMU (which is, let's say, quite complicated)? If I do, does it need to be in the rotational center of robot (so that no centrifugal forces distort the measurements)? I thought that mounting IMU anywhere was supported by robot_localization (also Documentation says it), but now I'm not completely sure about that... Thank you for your responses
Note: frame_id of imu/data messages header is imu_data and no misleading data (like odom->base_footprint tf, base_link->imu_data tf or /odometry/filtered) is included in the bag file.
Note ...