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;
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);
}
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;
}
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 "";
}
}
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.