Ask Your Question
0

dynamic tf broadcaster updates stale/intermittent in tf view_frames

asked 2019-09-15 16:51:17 -0600

Jimboner gravatar image

updated 2019-09-16 05:27:28 -0600

gvdhoorn gravatar image

Hello everybody,

Description: Ubuntu 18.04.2 LTS, Jetson nano, Ros melodic

My robot platform is an inverted pendulum, and trying to implement a dynamic transform between the base_link (centered between wheels and fixed w/r/t gravity vector), and base_frame (co-located with base_link, but includes the pitch measurement). The sensor that is providing the attitude measurement also produces a visual odometry measurement, so I have an odom_listener node that will send an updated base_link to base_frame transform, and a base_link to camera_odom_frame transform on every new measurement.

However, when I run "tf view_frames", I will only infrequently see the base_link to base_frame or camera_odom_frame nodes, and if I do, the measurements are typically many seconds old. If I run "tf tf_echo base_link base_frame", I can usually see tf broadcast updates, but interestingly, they can show the same "At time" values for multiple broadcasts:

$ rosrun tf tf_echo base_link base_frame 

At time 1568583863.323
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.002, 0.000, 1.000]
            in RPY (radian) [0.000, 0.003, 0.000]
            in RPY (degree) [0.000, 0.186, 0.000] 

At time 1568583863.323
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.002, 0.000, 1.000]
            in RPY (radian) [0.000, 0.003, 0.000]
            in RPY (degree) [0.000, 0.186, 0.000]

Looking at the code in the odom_listener.cpp, I print updates regularly, and can see the code is getting called with a much higher frequency that I'm observing updates in view_frame or tf_echo. Any thoughts why?

#include "ros/ros.h"
#include <tf/transform_broadcaster.h>
#include "nav_msgs/Odometry.h"

/**
 * http://wiki.ros.org/evarobot_odometry/Tutorials/indigo/Writing%20a%20Simple%20Subscriber%20for%20Odometry
 * https://answers.ros.org/question/11545/plotprint-rpy-from-quaternion/#17106
 */

/**
 * on VO message from T265, updates base_link to base_frame transform used
 * by GPS and laser.  Also transforms the VO data to the base_link frame
 */
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    // Inspect incoming message
    ROS_INFO("Seq: [%d]", msg->header.seq);
    //ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z);
    //ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    //ROS_INFO("Vel-> Linear: [%f], Angular: [%f]", msg->twist.twist.linear.x,msg->twist.twist.angular.z);

    // Construct tf::Quaterion
    tf::Quaternion quat(msg->pose.pose.orientation.x, 
        msg->pose.pose.orientation.y, 
        msg->pose.pose.orientation.z, 
        msg->pose.pose.orientation.w);

    // Get RPY from Quaternion
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    // Display converted RPY
    ROS_INFO("published rpy angles: roll=%f pitch=%f yaw=%f", roll, pitch, yaw);

    //Update tf::Quaternion with pitch only
    tf::Quaternion qPitchOnly;
    qPitchOnly.setRPY(0.0, pitch, 0.0);

    //base_link to base_frame transformation
    tf::TransformBroadcaster broadcaster;
    ros::Time currentTime = ros::Time::now();

    double secs = currentTime.toSec();
    ROS_INFO("currentTime: [%f]", secs);

    broadcaster.sendTransform(
        tf::StampedTransform(
            tf::Transform(qPitchOnly, tf::Vector3(0.0, 0 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
4

answered 2019-09-16 05:26:54 -0600

gvdhoorn gravatar image

updated 2019-09-16 05:28:30 -0600

Your code shows this:

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    [..]

    tf::TransformBroadcaster broadcaster;

    [..]

    broadcaster.sendTransform(..);

    [..]
}

you're (re)creating the tf::TransformBroadcaster in every odomCallback(..).

As broadcasters essentially wrap regular ros::Publishers, the same requirements and constraints apply: never create Publishers inside the body of callbacks.

Setting up a publication takes time. Setting up subcriptions takes time. There is almost no time between creating the TransformBroadcaster and sending transforms with it in your code. And even if the subscription is setup, the Publisher disappears almost immediately after the frames have been broadcast, which increases the chance that the data never actually reaches the subscriber(s).

Create the TransformBroadcaster in a scope that has a longer lifetime than your callback -- such as your main(..), or use a class to wrap all of this -- and I expect things to start working properly.

edit flag offensive delete link more

Comments

Gvdhoorn,

Thanks for the quick response, wrapping it into a class works great!

Jim

Jimboner gravatar imageJimboner ( 2019-09-19 22:53:31 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-09-15 16:51:17 -0600

Seen: 24 times

Last updated: Sep 16