dynamic tf broadcaster updates stale/intermittent in tf view_frames
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 ...