From 7a4f831edd4fc494eeee61fd10e8321e2f64903d Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Mon, 15 Mar 2021 12:19:32 +0900 Subject: [PATCH] add mapTFName() function to fix frame-id of messages --- src/stageros.cpp | 102 +++++++++++++++++++++++++++++++++++++---------- 1 file changed, 80 insertions(+), 22 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 39d040a..69aa7c0 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -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; @@ -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) { @@ -476,9 +534,9 @@ StageNode::WorldCallback() } if (robotmodel->lasermodels.size() > 1) - msg.header.frame_id = mapName("base_laser_link", r, s, static_cast(robotmodel->positionmodel)); + msg.header.frame_id = mapTFName("base_laser_link", r, s, static_cast(robotmodel->positionmodel)); else - msg.header.frame_id = mapName("base_laser_link", r, static_cast(robotmodel->positionmodel)); + msg.header.frame_id = mapTFName("base_laser_link", r, static_cast(robotmodel->positionmodel)); msg.header.stamp = sim_time; robotmodel->laser_pubs[s].publish(msg); @@ -493,19 +551,19 @@ StageNode::WorldCallback() if (robotmodel->lasermodels.size() > 1) tf.sendTransform(tf::StampedTransform(txLaser, sim_time, - mapName("base_link", r, static_cast(robotmodel->positionmodel)), - mapName("base_laser_link", r, s, static_cast(robotmodel->positionmodel)))); + mapTFName("base_link", r, static_cast(robotmodel->positionmodel)), + mapTFName("base_laser_link", r, s, static_cast(robotmodel->positionmodel)))); else tf.sendTransform(tf::StampedTransform(txLaser, sim_time, - mapName("base_link", r, static_cast(robotmodel->positionmodel)), - mapName("base_laser_link", r, static_cast(robotmodel->positionmodel)))); + mapTFName("base_link", r, static_cast(robotmodel->positionmodel)), + mapTFName("base_laser_link", r, static_cast(robotmodel->positionmodel)))); } //the position of the robot tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(), sim_time, - mapName("base_footprint", r, static_cast(robotmodel->positionmodel)), - mapName("base_link", r, static_cast(robotmodel->positionmodel)))); + mapTFName("base_footprint", r, static_cast(robotmodel->positionmodel)), + mapTFName("base_link", r, static_cast(robotmodel->positionmodel)))); // Get latest odometry data // Translate into ROS message format and publish @@ -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(robotmodel->positionmodel)); + odom_msg.header.frame_id = mapTFName("odom", r, static_cast(robotmodel->positionmodel)); odom_msg.header.stamp = sim_time; robotmodel->odom_pub.publish(odom_msg); @@ -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(robotmodel->positionmodel)), - mapName("base_footprint", r, static_cast(robotmodel->positionmodel)))); + mapTFName("odom", r, static_cast(robotmodel->positionmodel)), + mapTFName("base_footprint", r, static_cast(robotmodel->positionmodel)))); // Also publish the ground truth pose and velocity Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose(); @@ -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(robotmodel->positionmodel)); + ground_truth_msg.header.frame_id = mapTFName("odom", r, static_cast(robotmodel->positionmodel)); ground_truth_msg.header.stamp = sim_time; robotmodel->ground_truth_pub.publish(ground_truth_msg); @@ -605,9 +663,9 @@ StageNode::WorldCallback() } if (robotmodel->cameramodels.size() > 1) - image_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel)); + image_msg.header.frame_id = mapTFName("camera", r, s, static_cast(robotmodel->positionmodel)); else - image_msg.header.frame_id = mapName("camera", r,static_cast(robotmodel->positionmodel)); + image_msg.header.frame_id = mapTFName("camera", r,static_cast(robotmodel->positionmodel)); image_msg.header.stamp = sim_time; robotmodel->image_pubs[s].publish(image_msg); @@ -663,9 +721,9 @@ StageNode::WorldCallback() } if (robotmodel->cameramodels.size() > 1) - depth_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel)); + depth_msg.header.frame_id = mapTFName("camera", r, s, static_cast(robotmodel->positionmodel)); else - depth_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel)); + depth_msg.header.frame_id = mapTFName("camera", r, static_cast(robotmodel->positionmodel)); depth_msg.header.stamp = sim_time; robotmodel->depth_pubs[s].publish(depth_msg); } @@ -686,18 +744,18 @@ StageNode::WorldCallback() if (robotmodel->cameramodels.size() > 1) tf.sendTransform(tf::StampedTransform(tr, sim_time, - mapName("base_link", r, static_cast(robotmodel->positionmodel)), - mapName("camera", r, s, static_cast(robotmodel->positionmodel)))); + mapTFName("base_link", r, static_cast(robotmodel->positionmodel)), + mapTFName("camera", r, s, static_cast(robotmodel->positionmodel)))); else tf.sendTransform(tf::StampedTransform(tr, sim_time, - mapName("base_link", r, static_cast(robotmodel->positionmodel)), - mapName("camera", r, static_cast(robotmodel->positionmodel)))); + mapTFName("base_link", r, static_cast(robotmodel->positionmodel)), + mapTFName("camera", r, static_cast(robotmodel->positionmodel)))); sensor_msgs::CameraInfo camera_msg; if (robotmodel->cameramodels.size() > 1) - camera_msg.header.frame_id = mapName("camera", r, s, static_cast(robotmodel->positionmodel)); + camera_msg.header.frame_id = mapTFName("camera", r, s, static_cast(robotmodel->positionmodel)); else - camera_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel)); + camera_msg.header.frame_id = mapTFName("camera", r, static_cast(robotmodel->positionmodel)); camera_msg.header.stamp = sim_time; camera_msg.height = cameramodel->getHeight(); camera_msg.width = cameramodel->getWidth();