flightgoggles_ros_bridge/src/ROSClient/ROSClient.cpp
// Subscribe to all TF messages to avoid lag.
Subscribe to all TF messages to avoid lag.
Subscribe to all TF messages to avoid lag .
void ROSClient::tfCallback(tf2_msgs::TFMessage::Ptr msg){
    bool found_transform = false;
    for (auto transform : msg->transforms){
        if (transform.child_frame_id == "uav/imu"){
            found_transform = true;
        }
    }
    if (!found_transform) return;
    if (numSimulationStepsSinceLastRender_ >= numSimulationStepsBeforeRenderRequest_){
        geometry_msgs::TransformStamped camLeftTransform;
        try{
            camLeftTransform = tfBuffer_.lookupTransform("world", "uav/camera/left/internal_nwu", ros::Time(0));
        } catch (tf2::TransformException &ex) {
            ROS_WARN("Could NOT find transform for /uav/camera/left/nwu: %s", ex.what());
        }
        Transform3 camLeftPose = tf2::transformToEigen(camLeftTransform);
        flightGoggles.setCameraPoseUsingROSCoordinates(camLeftPose, 0);
        if (render_stereo) {
            geometry_msgs::TransformStamped camRightTransform;
            try{
                camRightTransform = tfBuffer_.lookupTransform("world", "uav/camera/right/internal_nwu", camLeftTransform.header.stamp);
            } catch (tf2::TransformException &ex) {
                ROS_WARN("Could NOT find transform for /uav/camera/right/nwu: %s", ex.what());
            }
            Transform3 camRightPose = tf2::transformToEigen(camRightTransform);
            flightGoggles.setCameraPoseUsingROSCoordinates(camRightPose, 1);
        }
        flightGoggles.state.ntime = camLeftTransform.header.stamp.toNSec();
        flightGoggles.requestRender();
        flightGoggles.state.scene.reinitialize &= false;
        scene_params_.reinitialize &= false;
        numSimulationStepsSinceLastRender_ = 0;
    } else {
        numSimulationStepsSinceLastRender_++;
    }
}
void ROSClient :: tfCallback ( tf2_msgs :: TFMessage :: Ptr msg ) { bool found_transform = false ; for ( auto transform : msg -> transforms ) { if ( transform . child_frame_id == " " ) { found_transform = true ; } } if ( ! found_transform ) return ; if ( numSimulationStepsSinceLastRender_ >= numSimulationStepsBeforeRenderRequest_ ) { geometry_msgs :: TransformStamped camLeftTransform ; try { camLeftTransform = tfBuffer_ . lookupTransform ( " " , " " , ros :: Time ( 0 ) ) ; } catch ( tf2 :: TransformException & ex ) { ROS_WARN ( " " , ex . what ( ) ) ; } Transform3 camLeftPose = tf2 :: transformToEigen ( camLeftTransform ) ; flightGoggles . setCameraPoseUsingROSCoordinates ( camLeftPose , 0 ) ; if ( render_stereo ) { geometry_msgs :: TransformStamped camRightTransform ; try { camRightTransform = tfBuffer_ . lookupTransform ( " " , " " , camLeftTransform . header . stamp ) ; } catch ( tf2 :: TransformException & ex ) { ROS_WARN ( " " , ex . what ( ) ) ; } Transform3 camRightPose = tf2 :: transformToEigen ( camRightTransform ) ; flightGoggles . setCameraPoseUsingROSCoordinates ( camRightPose , 1 ) ; } flightGoggles . state . ntime = camLeftTransform . header . stamp . toNSec ( ) ; flightGoggles . requestRender ( ) ; flightGoggles . state . scene . reinitialize &= false ; scene_params_ . reinitialize &= false ; numSimulationStepsSinceLastRender_ = 0 ; } else { numSimulationStepsSinceLastRender_ ++ ; } }
Subscribe to all TF messages to avoid lag.
Subscribe to all TF messages to avoid lag .
//geometry_msgs::TransformStamped world_to_uav; // Check if TF message is for world->uav/imu // This is output by dynamics node. //world_to_uav = transform; // Skip if do not have transform // Skip every other transform to get an update rate of 60hz // Get transform for left camera // Get transform for second camera (right) // Update timestamp of state message (needed to force FlightGoggles to rerender scene) // request render // reset scene reinitialize