Unable to transform PointCloud2 with TF StampedTransform

edit

I have seen many questions along these lines, but this is a unique case. We have a solid-state LiDAR that is publishing PointCloud2 messages at 100Hz and a RealSense T265 tracking camera that publishes odom transforms at 200Hz. I have tried literally every hack in the book to integrate the T265 poses with the LiDAR points, but nothing is working.

Now, I have a launch file that contains the following static transform publisher:

<node pkg="tf" type="static_transform_publisher" name="t265_to_lidar" args="0 0 0 0 0 0 /camera_odom_frame /lidar_frame 1000"/>

If I do not publish this static transform, ROS will not see either the camera_odom_frame or the lidar_frame on the callback. Here is the code being used for the ROS node:

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>
#include <opencv2/opencv.hpp>

#include <geometry_msgs/PointStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/message_filter.h>
#include <message_filters/subscriber.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

using namespace Eigen; 
using namespace std;

tf::TransformListener *tf_listener;
ros::Publisher publisher;

void orient_cloud(const sensor_msgs::PointCloud2 &cloud_msg) {

    tf::StampedTransform transform;

    try{
      tf_listener->lookupTransform("camera_odom_frame", "lidar_frame",
                               (cloud_msg).header.stamp, transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }

    Matrix4f m;
    pcl_ros::transformAsMatrix(transform, m);

    sensor_msgs::PointCloud2 output;
    pcl_ros::transformPointCloud(m, cloud_msg, output);

    publisher.publish (output);

}

int main(int argc, char **argv) {

    ros::init(argc, argv, "captureLiDARIMU");
    ros::NodeHandle nh;

    tf_listener = new tf::TransformListener();

    ros::Subscriber subscriber = nh.subscribe("/lidar/points", 100, orient_cloud);
    publisher = nh.advertise<sensor_msgs::PointCloud2>("/lidar/transformed_cloud", 100);


    ros::spin();

    return 0;

}

First thing you are probably wondering is why I implemented an Eigen 4x4 matrix. This was to troubleshoot the StampedTransform, because it never changes. It stays at the initial "zero" pose, regardless of moving the T265 around. In other words, the tf_listener->lookupTransform is never giving anything other than the initial pose.

What I don't understand is why ROS doesn't pick up on the T265's camera_odom_frame transforms. If I separately invoke "rostopic echo /camera/odom/sample" which has the odom_frame as its parent, it will show the changing poses as I move around. I originally attempted this without the static transform publisher, but then, as I mentioned above, I receive continual "lidar_frame passed to lookupTransform argument source_frame does not exist" messages on the callback. This also is confusing, because the sensor_msg from the LiDAR is being handed over directly to the callback. Why wouldn't it be able to see its parent frame?

We also attempted to convert this to a message_filter model, but there are issues in Noetic which prevent using PointCloud2 as data types. I haven't figured out that problem yet, so trying to keep things clean and simple. I do hope someone has an idea what the problem is here, because functionally, this should be very straightforward.