This is a general issue that applies to any sensor whose pose data is reported in a world-fixed frame, yet is mounted at some location on the vehicle other than its centroid. Fundamentally, the issue is that we define the sensor's offset from the vehicle (via URDFs or static_transform_publisher
) as a transform from the vehicle origin (e.g., base_link) to the sensor frame (e.g., depth_sensor_frame), even if that sensor is measuring data in a world-fixed frame.
At first, my reaction to this problem was that the transform from the sensor to the world frame (e.g., depth_sensor_frame->base_link->odom) would suffice, but this is incorrect. If we applied the transform from the sensor straight to the world frame, we would continually be adding the vehicle's full position to each sensor measurement. For example, if we had a depth sensor that measured 10 meters, and we applied the transform from depth->base_link->odom, and the vehicle was already at 10 meters of depth, then the final measurement would read 20. What we really need in this case is the offset from the vehicle's centroid (as defined by the base_link->depth transform), rotated by the vehicle's current orientation.
In my own work, I have gotten around this by simply defining the odom->depth_sensor_frame directly as a static transform, but that fails to handle the effect vehicle's current orientation on the offset.
As a more in-depth example, let's say we have a depth sensor on the rear end of a UUV. The depth sensor is measuring data in the world frame (odom or map). Right now, the poseCallback function attempts to transform pose data from the reported message frame_id to the odom (or whatever your world_frame parameter is set to) frame. So for our depth sensor, you would need to define a transform from odom to the depth_sensor_frame that technically has the transform from base_link->depth_sensor_frame in it. This is wrong, as it won't handle the vehicle's orientation. What we really want to do is this:
First, we define the transform of the sensor from base_link->depth_sensor_frame statically as usual. When we get a new measurement for the depth sensor, we need to rotate the base_link->depth_sensor_frame transform by the odom->base_link orientation (but we should not apply the world position!). This will give us the rotated offset of the depth sensor w.r.t. the the vehicle center.
We then apply that transform to the depth sensor measurement itself, and fuse it.
So, for example, if we have a depth sensor that has a X offset of -1.5 meters from the vehicle center, and a +Z offset of 0.5 meters, then when the vehicle is level, then (assuming our depth sensor reads negative values as it gets deeper, so -Z is down), we just subtract the Z offset from the raw measurement to get the vehicle center measurement. However, if the vehicle is pitched down, then the "true" vertical offset in the odom frame is a function of both the X offset and the Z offset (this is obviously true when the vehicle is level as well, but in that case the pitch is 0, and so the trigonometry would eliminate the X offset's effect). Therefore, we need a combination of the base_link->depth_sensor_frame transform and the vehicle's current orientation, which is specified by part of the odom->base_link transform.