Remapping does not work for TransformBroadcaster
Hello folks!
For a specific case where I want to publish odom tfs for a bunch of simulated robots, I am trying to remap the "/tf" topic to robot specific topics such as /robot1tf, /robot2tf and so on. More precisely, somewhere along a control loop that simulates all my robots in one common process, I do this in order to feed amcl:
tf::TransformBroadcaster* tfBroadcaster;
void ROSInterface::sendOdomData(const Vector<OdomData> &odomData)
{
for (uint i = 0; i < odomData.size(); i++) // process odomData for every simulated robot
{
// Publish the odom message.
nav_msgs::Odometry odom;
odom.header.stamp = ros::Time::now();
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.pose.position.x = odomData[i].x;
odom.pose.pose.position.y = odomData[i].y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odomData[i].theta);
odom.twist.twist.linear.x = odomData[i].vx;
odom.twist.twist.linear.y = odomData[i].vy;
odom.twist.twist.angular.z = odomData[i].omega;
odomPub[i].publish(odom);
// Publish the odom transform with the TransformBroadcaster.
geometry_msgs::TransformStamped odom_tf;
odom_tf.header = odom.header;
odom_tf.child_frame_id = "base_link";
odom_tf.transform.translation.x = odomData[i].x;
odom_tf.transform.translation.y = odomData[i].y;
odom_tf.transform.translation.z = 0.0;
odom_tf.transform.rotation = odom.pose.pose.orientation;
tfBroadcaster->sendTransform(odom_tf);
}
}
See how I have a vector of odomPub for every robot? I can set that up easily like so:
std::vector<ros::Publisher> odomPub;
for (uint i = 0; i < robots.size(); i++)
odomPub.push_back(nh->advertise<nav_msgs::Odometry>(std::string(robots[i].name + "/odom"), 1));
But with the TransformBroadcaster this is pointless and all published transforms are meshed together in one "/tf" topic where it is impossible to distinguish them from each other.
After a lot of googeling, I came across the idea that I could use a separate node handle for every simulated robot and instantiate each node handle with a specific mapping like so:
ros::M_string remappings;
remappings.insert(std::make_pair("/tf", "/robot1_tf"));
remappings.insert(std::make_pair("tf", "/robot1_tf"));
nh1 = new ros::NodeHandle("devices", remappings);
but no. It doesn't do it. The TransformBroadcaster still publishes on "/tf" as if the mappings weren't there. The mapping does work though on topics that I advertise myself. Is there something I am not doing right? Is there a simple way to fix this or remap /tf some other way?
So far the only way I have been able to successfully remap /tf is using remap in a launch file,
<remap from="/tf" to="/$(arg robot_id)_tf"/>
but this would be an absolute nightmare. The number of simulated robots can change any time, ideally even during runtime. Even if I restart the whole simulation, I would then have to dynamically create a launch file that launches a separate node for every robot so that I can remap their /tf individually, not even speaking of getting the odom data from the simulation process into the separate process of every launched node.
Any ideas are welcome.
Asked by Marcell on 2023-07-23 09:06:45 UTC
Comments