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:
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