TF name will start '/' when using multi robot configuration due to invalid specification of mapName() function for frame_id

[sktometometo]

I am using stage_ros with move_base_multi_robot.launch in navigation_tutorials. The launch file does not work properly since tf configuration is invalid.

One of the invalid tf configuration is that frame_id fields of messsages published from stage_ros start with '/' when using multi-robot configuration. ( this is already reported in #68 )

This issue seems comes from mapName() function according to

stage_ros/src/stageros.cpp

Lines 478 to 481 in c3abc19

	if (robotmodel->lasermodels.size() > 1)
	msg.header.frame_id = mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
	else
	msg.header.frame_id = mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel));

.
And seeing

stage_ros/src/stageros.cpp

Lines 172 to 227 in c3abc19

	// since stageros is single-threaded, this is OK. revisit if that changes!
	const char *
	StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
	{
	//ROS_INFO("Robot %lu: Device %s", robotID, name);
	bool umn = this->use_model_names;
	if ((positionmodels.size() > 1 ) || umn)
	{
	static char buf[100];
	std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
	if ((found==std::string::npos) && umn)
	{
	snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
	}
	else
	{
	snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
	}
	return buf;
	}
	else
	return name;
	}
	const char *
	StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const
	{
	//ROS_INFO("Robot %lu: Device %s:%lu", robotID, name, deviceID);
	bool umn = this->use_model_names;
	if ((positionmodels.size() > 1 ) || umn)
	{
	static char buf[100];
	std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
	if ((found==std::string::npos) && umn)
	{
	snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
	}
	else
	{
	snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
	}
	return buf;
	}
	else
	{
	static char buf[100];
	snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
	return buf;
	}
	}

, when the size of positionmodels is lager than 1, frame_id is resolved as like /robot_0/base_laser_link.

This specification is valid for topicnames. But for frame_id, this is invalid.

[imranlivt]

I have fixed this bug in a branch. have tested it on Melodic. Please have a look if anybody is still facing this issue
stage_ros

[leander-dsouza]

@imranlivt your pull request has helped me to solve this issue when it arose from porting from melodic to noetic.

This is the error that was printed on the console when I tried to subscribe /robot_0/base_pose_ground_truth from the /map frame in RViz:

Error transforming pose 'Pose' from frame '/robot_0/odom' to frame 'map'

Thank you very much!.

[davidbsp]

@wjwwood can you please merge this asap? This is breaking the compatibility of many multi-robot packages that rely on stage in the latest ROS1 versions.