Robotics StackExchange | Archived questions

No TF is being published

I have some strange problem and I do not why this is happening. I have a node that publishes the Transfom message into /mapbasetf 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;
}

Asked by stevemartin on 2019-02-11 05:20:19 UTC

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?

Asked by gvdhoorn on 2019-02-11 05:40:22 UTC

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

Asked by stevemartin on 2019-02-11 06:14:13 UTC

Answers