No TF is being published

asked 2019-02-11 04:20:19 -0500

stevemartin gravatar image

I have some strange problem and I do not why this is happening. I have a node that publishes the Transfom message into /map_base_tf topic and I can confirm that the messages are being published into that topic.

Then I have a second node and it subscribes to that topic and does some inverse stuff and publishes the TF. I can confirm that this node is being subscribed to that topic but it does not publish the TF nor it notifies me that the callback is being called:

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Transform.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/OccupancyGrid.h>



void callback(const geometry_msgs::TransformStamped::ConstPtr& msg)
{
    ROS_INFO("Callback %s", "callback");
    tf2_ros::TransformBroadcaster tfb;
    tf2::Transform latest_tf(tf2::Quaternion(msg->transform.rotation.x,msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w), 
                            tf2::Vector3(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z));;

    latest_tf = latest_tf.inverse();
    geometry_msgs::TransformStamped map_to_odom;
    map_to_odom.header.stamp = msg->header.stamp;
    map_to_odom.header.frame_id = "map";
    map_to_odom.child_frame_id = "odom";
    map_to_odom.transform.translation.x = latest_tf.getOrigin().getX();
    map_to_odom.transform.translation.y = latest_tf.getOrigin().getY();
    map_to_odom.transform.translation.z = latest_tf.getOrigin().getZ();
    map_to_odom.transform.rotation.x = latest_tf.getRotation().getX();
    map_to_odom.transform.rotation.y = latest_tf.getRotation().getY();
    map_to_odom.transform.rotation.z = latest_tf.getRotation().getZ();
    map_to_odom.transform.rotation.w = latest_tf.getRotation().getW();
    tfb.sendTransform(map_to_odom);

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "map_to_odom_node");
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe("/map_base_tf", 100, callback);
    ros::spin();
    return 0;
}
edit retag flag offensive close merge delete

Comments

Just curious:

Then I have a second node and it subscribes to that topic and does some inverse stuff and publishes the TF.

TF is capable of providing you with "inverse transforms". Is your node doing something more, or special?

gvdhoorn gravatar image gvdhoorn  ( 2019-02-11 04:40:22 -0500 )edit

@gvdhoorn It only inverses the TF and publishes. Any idea why my code does not work?

stevemartin gravatar image stevemartin  ( 2019-02-11 05:14:13 -0500 )edit