ROS and renaming TF frames

Report crashes, strange behaviour, or apparent bugs
Post Reply
fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

ROS and renaming TF frames

Post by fferri »

When using simExtROS_enablePublisher with topic='/tf' and specifying an auxString argument such as "child%parent" broadcasted frames are renamed. This is useful to match the configuration on the real robot, and especially useful when using multiple instances of the same robot (using tf_prefix), because one can output frames such as "robot1/laser%robot1/base_link".

The problem is that other publishers do not use the renamed frame id in the message header. So, for example, if publishing a point cloud with respect to vrep object "laser_ref", but I renamed that frame to "robot1/laser", the published pointcloud gets published in a non-existing "laser_ref" TF frame.

To solve this, I modified ROS_server.cpp, adding a map<string,string> keeping track of such changes:

Code: Select all

#include <map>

std::map<std::string, std::string> renamedFrame;
then modifying the objNameToFrameId function to lookup in this map before trying to create a TF frame id based on vrep object name:

Code: Select all

static std::string objNameToFrameId(const std::string & name) 
{
	// check if there is a remapping
	std::map<std::string, std::string>::iterator it = renamedFrame.find(name);
	if(it != renamedFrame.end())
		return(renamedFrame[name]);

	std::string slash("/");
	if (name[0] == '/') 
		return(name);
	
	return(slash+name);
}
To populate this map, I had to modify the method splitting the auxString wrt '%' symbol (around line 1605) to keep the old object name that would be used for frame id, and later, create the mapping between that name and the name specified in the auxString of simExtROS_enablePublisher:

Code: Select all

					std::string old_offspring_name = offspring_name;
					std::string old_parent_name = parent_name;

					if (publishers[pubI].auxStr.size()>0)
					{
						// auxstr can be used to specify the names, in case the handles
						// do not correspond to the exact names we want to use in
						// frame_ids...
						// Syntax "offspring_name%parent_name"
						size_t pos = publishers[pubI].auxStr.find_first_of("%");
						if ((pos == std::string::npos) || (pos > 0)) {
							offspring_name = publishers[pubI].auxStr.substr(0,pos);
						}
						if ((pos != std::string::npos) && (pos != publishers[pubI].auxStr.size()-1)) {
							parent_name = publishers[pubI].auxStr.substr(pos+1,std::string::npos);
						}
					}
					offspring_name = objNameToFrameId(offspring_name);
					parent_name = objNameToFrameId(parent_name);

					if (publishers[pubI].auxStr.size()>0)
					{
						renamedFrame[old_offspring_name] = offspring_name;
						renamedFrame[old_parent_name] = parent_name;
					}
This should theoretically have worked.
What happens instead, is that the publisher is enabled before the mapping is created, and the field header.frame_id of the point cloud (or any other message with header) is set only when enabling the publisher for the first time.

So I set the field header.frame_id at each stream. I define a new function looking up the remapping given the object id (instead of the object name):

Code: Select all

static std::string objIdToFrameId(int objId) 
{
	simChar* pname=simGetObjectName(objId);
	if (pname!=NULL)
	{
		std::string objName(pname);
		std::string ret = objNameToFrameId(objName);
		simReleaseBuffer(pname);
		return ret;
	}
	else
	{
		return "";
	}
}
Then, for example, in ROS_server::streamLaserCloud, I add:

Code: Select all

pcd->pointcloud.header.frame_id = objIdToFrameId(pub.auxInt1);

I'm not submitting a proper patch, because this looks a bit quirky.
If I had more time, I would refactor some parts of ROS_server.cpp such that this can be handled better.
However, maybe you know better why ROS_server.cpp is the way it is, and you can improve my solution to suit the current design of ROS_server.cpp.

Hope this helps anyway.

Post Reply