Long response. Sorry. I tried to use i_override_local_transform as follows. Here is the bit of my launch file that creates VIO specific parameters:
camera_to_base_link = lookup_camera_to_base_link_transform()
vio_parameters.update({
"vio.i_child_frame_id": "base_link",
"vio.i_override_local_transform": True,
"vio.i_local_transform_x": camera_to_base_link.translation.x,
"vio.i_local_transform_y": camera_to_base_link.translation.y,
"vio.i_local_transform_z": camera_to_base_link.translation.z,
"vio.i_local_transform_qx": camera_to_base_link.rotation.x,
"vio.i_local_transform_qy": camera_to_base_link.rotation.y,
"vio.i_local_transform_qz": camera_to_base_link.rotation.z,
"vio.i_local_transform_qw": camera_to_base_link.rotation.w,
})
with
def lookup_camera_to_base_link_transform():
import time
import rclpy
from rclpy.node import Node as RclpyNode
from rclpy.time import Time
from tf2_ros import Buffer, TransformListener, TransformException
owns_rclpy = not rclpy.ok()
if owns_rclpy:
rclpy.init(args=None)
node = RclpyNode('oak_vio_local_transform_lookup')
buffer = Buffer()
listener = TransformListener(buffer, node)
transform = None
deadline = time.time() + 10.0
while time.time() < deadline:
rclpy.spin_once(node, timeout_sec=0.2)
try:
# "camera-to-robot transform, so VIO publishes the robot pose
# directly" (per Luxonis) - the pose of base_link as seen from the
# camera frame, i.e. how to convert a camera-frame pose into a
# base_link-frame one.
transform = buffer.lookup_transform('base_link', 'camera', Time())
break
except TransformException:
continue
node.destroy_node()
if owns_rclpy:
rclpy.shutdown()
if transform is None:
raise RuntimeError(
"use_vio:=true needs the base_link -> camera transform, but it "
"never appeared on /tf within 10s. Is robot_state_publisher "
"(with the robot's URDF) running?"
)
return transform.transform
The robot is still not correctly oriented. I can't paste in screenshots, but here is a description. Without overriding the local transform, the robot is upside down, below the ground, the front tilted up (because upside down) by an amount that seems to correspond to the tilt of the camera. The front of the robot is more or less at z=0. With the code above, the robot is now above ground, still upside down and tilted up but also rolled (by maybe 10deg) and yawed (by 90deg) relative to the previous case. I have tried to inverse the transform (camera to base_link) and that obviously changes things but still wrongly.
Any ideas?
Cheers,
Fred
