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

Fixed odom message with empty child_frame_id field #59

Open
wants to merge 3 commits into
base: lunar-devel
Choose a base branch
from
Open

Fixed odom message with empty child_frame_id field #59

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
707fca9
Fixed odom message with empty child_frame_id field
mtlazaro Jan 7, 2020
fa95bd4
Stage version 4.3.0 required
mtlazaro Jan 7, 2020
eab9e01
added mapNameFrame to remove first '/' from frame_ids (tf2 convention)
May 6, 2021
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
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ find_package(catkin REQUIRED

find_package(Boost REQUIRED COMPONENTS system thread)

find_package(stage REQUIRED)
find_package(stage 4.3.0 REQUIRED)

include_directories(
${catkin_INCLUDE_DIRS}
Expand Down
104 changes: 81 additions & 23 deletions src/stageros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,9 @@ 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 * mapNameFrame(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;
const char * mapNameFrame(const char *name, size_t robotID, Stg::Model* mod) const;

tf::TransformBroadcaster tf;

// Last time that we received a velocity command
Expand Down Expand Up @@ -196,6 +198,32 @@ StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
return name;
}

const char *
StageNode::mapNameFrame(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
{
Expand Down Expand Up @@ -226,6 +254,35 @@ StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model
}
}

const char *
StageNode::mapNameFrame(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 +533,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 = mapNameFrame("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 = mapNameFrame("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 +550,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))));
mapNameFrame("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapNameFrame("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))));
mapNameFrame("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapNameFrame("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))));
mapNameFrame("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapNameFrame("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

// Get latest odometry data
// Translate into ROS message format and publish
Expand All @@ -521,7 +578,8 @@ 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 = mapNameFrame("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
odom_msg.child_frame_id = mapNameFrame("base_footprint", 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))));
mapNameFrame("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapNameFrame("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 = mapNameFrame("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 = mapNameFrame("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 = mapNameFrame("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 = mapNameFrame("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 = mapNameFrame("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))));
mapNameFrame("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapNameFrame("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))));
mapNameFrame("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapNameFrame("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 = mapNameFrame("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 = mapNameFrame("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