Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add mapTFName() function to fix frame-id of messages #70

Open
wants to merge 1 commit into
base: lunar-devel
Choose a base branch
from
Open

Add mapTFName() function to fix frame-id of messages #70

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
102 changes: 80 additions & 22 deletions src/stageros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,8 @@ class StageNode
// is true, an unaltered copy of the name is returned.
const char *mapName(const char *name, size_t robotID, Stg::Model* mod) const;
const char *mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;
const char *mapTFName(const char *name, size_t robotID, Stg::Model* mod) const;
const char *mapTFName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;

tf::TransformBroadcaster tf;

Expand Down Expand Up @@ -226,6 +228,62 @@ StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model
}
}

const char *
StageNode::mapTFName(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::mapTFName(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;
}
}

void
StageNode::ghfunc(Stg::Model* mod, StageNode* node)
{
Expand Down Expand Up @@ -476,9 +534,9 @@ StageNode::WorldCallback()
}

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

msg.header.stamp = sim_time;
robotmodel->laser_pubs[s].publish(msg);
Expand All @@ -493,19 +551,19 @@ StageNode::WorldCallback()

if (robotmodel->lasermodels.size() > 1)
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
mapTFName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapTFName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
else
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
mapTFName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapTFName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
}

//the position of the robot
tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
sim_time,
mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
mapTFName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapTFName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

// Get latest odometry data
// Translate into ROS message format and publish
Expand All @@ -521,7 +579,7 @@ StageNode::WorldCallback()
//@todo Publish stall on a separate topic when one becomes available
//this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
//
odom_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
odom_msg.header.frame_id = mapTFName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
odom_msg.header.stamp = sim_time;

robotmodel->odom_pub.publish(odom_msg);
Expand All @@ -531,8 +589,8 @@ StageNode::WorldCallback()
tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, odomQ);
tf::Transform txOdom(odomQ, tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
mapTFName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapTFName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

// Also publish the ground truth pose and velocity
Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose();
Expand Down Expand Up @@ -568,7 +626,7 @@ StageNode::WorldCallback()
ground_truth_msg.twist.twist.linear.z = gvel.z;
ground_truth_msg.twist.twist.angular.z = gvel.a;

ground_truth_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
ground_truth_msg.header.frame_id = mapTFName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
ground_truth_msg.header.stamp = sim_time;

robotmodel->ground_truth_pub.publish(ground_truth_msg);
Expand Down Expand Up @@ -605,9 +663,9 @@ StageNode::WorldCallback()
}

if (robotmodel->cameramodels.size() > 1)
image_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
image_msg.header.frame_id = mapTFName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
else
image_msg.header.frame_id = mapName("camera", r,static_cast<Stg::Model*>(robotmodel->positionmodel));
image_msg.header.frame_id = mapTFName("camera", r,static_cast<Stg::Model*>(robotmodel->positionmodel));
image_msg.header.stamp = sim_time;

robotmodel->image_pubs[s].publish(image_msg);
Expand Down Expand Up @@ -663,9 +721,9 @@ StageNode::WorldCallback()
}

if (robotmodel->cameramodels.size() > 1)
depth_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
depth_msg.header.frame_id = mapTFName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
else
depth_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
depth_msg.header.frame_id = mapTFName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
depth_msg.header.stamp = sim_time;
robotmodel->depth_pubs[s].publish(depth_msg);
}
Expand All @@ -686,18 +744,18 @@ StageNode::WorldCallback()

if (robotmodel->cameramodels.size() > 1)
tf.sendTransform(tf::StampedTransform(tr, sim_time,
mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
mapTFName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapTFName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
else
tf.sendTransform(tf::StampedTransform(tr, sim_time,
mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
mapTFName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapTFName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

sensor_msgs::CameraInfo camera_msg;
if (robotmodel->cameramodels.size() > 1)
camera_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
camera_msg.header.frame_id = mapTFName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
else
camera_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
camera_msg.header.frame_id = mapTFName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
camera_msg.header.stamp = sim_time;
camera_msg.height = cameramodel->getHeight();
camera_msg.width = cameramodel->getWidth();
Expand Down