stella-cv / stella_vslam_ros

ROS package for stella_vslam
https://stella-cv.rtfd.io/en/latest/
Other
117 stars 77 forks source link

Transform failed: "odom" passed to lookupTransform argument source_frame does not exist. #50

Closed ryleymcc closed 2 years ago

ryleymcc commented 2 years ago

I am using realsense T265. It seems to work but I get the error in the title when I move the camera. I run the launch file.

<launch>
    <remap from="/t265/fisheye1/image_raw" to="/camera/image_raw"/>
    <include file="$(find realsense2_camera)/launch/rs_d400_and_t265.launch" >
        <arg name="enable_fisheye" value="true" />
    </include>
    <node name="openvslam" pkg="openvslam_ros" type="run_slam" args="-v $(find openvslam_ros)/src/setup/orb_vocab.fbow -c $(find openvslam_ros)/src/setup/fisheye.yaml" />
</launch>

it starts up and runs like normal but when I move the camera i get this terminal output.

[ERROR] [1634696431.547748506]: Transform failed: "odom" passed to lookupTransform argument source_frame does not exist. 
[ERROR] [1634696431.581208225]: Transform failed: "odom" passed to lookupTransform argument source_frame does not exist. 
[ERROR] [1634696431.615261292]: Transform failed: "odom" passed to lookupTransform argument source_frame does not exist. 
[ERROR] [1634696431.635600643]: Transform failed: "odom" passed to lookupTransform argument source_frame does not exist. 
[ERROR] [1634696431.731822265]: Transform failed: "odom" passed to lookupTransform argument source_frame does not exist.
...

line 46 in openvslam_ros.cc

    // Send map->odom transform. Set publish_tf to false if not using TF
    if (publish_tf_) {
        try {
            auto camera_to_odom = tf_->lookupTransform(camera_link_, odom_frame_, stamp, ros::Duration(0.0));
            Eigen::Affine3d camera_to_odom_affine = tf2::transformToEigen(camera_to_odom.transform);

            auto map_to_odom_msg = tf2::eigenToTransform(map_to_camera_affine * camera_to_odom_affine);
            auto transform_timestamp = stamp + ros::Duration(transform_tolerance_);
            map_to_odom_msg.header.stamp = transform_timestamp;
            map_to_odom_msg.header.frame_id = map_frame_;
            map_to_odom_msg.child_frame_id = odom_frame_;
            map_to_odom_broadcaster_->sendTransform(map_to_odom_msg);
        }
        catch (tf2::TransformException& ex) {
            ROS_ERROR("Transform failed: %s", ex.what());
        }
    }

So from terminal output it looks like odom_frame_does not exist?