From 707fca99a285633037e219cb9b9e977b72e85a03 Mon Sep 17 00:00:00 2001 From: Mayte Lazaro Date: Tue, 7 Jan 2020 13:09:21 +0100 Subject: [PATCH 1/3] Fixed odom message with empty child_frame_id field --- src/stageros.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/stageros.cpp b/src/stageros.cpp index 39d040a..057fc10 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -522,6 +522,7 @@ StageNode::WorldCallback() //this->odomMsgs[r].stall = this->positionmodels[r]->Stall(); // odom_msg.header.frame_id = mapName("odom", r, static_cast(robotmodel->positionmodel)); + odom_msg.child_frame_id = mapName("base_footprint", r, static_cast(robotmodel->positionmodel)); odom_msg.header.stamp = sim_time; robotmodel->odom_pub.publish(odom_msg); From fa95bd4a9918cd907b0b83d3ecc00e715662b565 Mon Sep 17 00:00:00 2001 From: Mayte Lazaro Date: Tue, 7 Jan 2020 15:34:07 +0100 Subject: [PATCH 2/3] Stage version 4.3.0 required --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d6bc35e..79a933b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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} From eab9e015ca8cd0417f0805daf776cf826d9fadbb Mon Sep 17 00:00:00 2001 From: mtlazaro Date: Thu, 6 May 2021 16:22:26 +0200 Subject: [PATCH 3/3] added mapNameFrame to remove first '/' from frame_ids (tf2 convention) --- src/stageros.cpp | 105 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 81 insertions(+), 24 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 057fc10..117c76c 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -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 @@ -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 { @@ -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) { @@ -476,9 +533,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 = mapNameFrame("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 = mapNameFrame("base_laser_link", r, static_cast(robotmodel->positionmodel)); msg.header.stamp = sim_time; robotmodel->laser_pubs[s].publish(msg); @@ -493,19 +550,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)))); + mapNameFrame("base_link", r, static_cast(robotmodel->positionmodel)), + mapNameFrame("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)))); + mapNameFrame("base_link", r, static_cast(robotmodel->positionmodel)), + mapNameFrame("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)))); + mapNameFrame("base_footprint", r, static_cast(robotmodel->positionmodel)), + mapNameFrame("base_link", r, static_cast(robotmodel->positionmodel)))); // Get latest odometry data // Translate into ROS message format and publish @@ -521,8 +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(robotmodel->positionmodel)); - odom_msg.child_frame_id = mapName("base_footprint", r, static_cast(robotmodel->positionmodel)); + odom_msg.header.frame_id = mapNameFrame("odom", r, static_cast(robotmodel->positionmodel)); + odom_msg.child_frame_id = mapNameFrame("base_footprint", r, static_cast(robotmodel->positionmodel)); odom_msg.header.stamp = sim_time; robotmodel->odom_pub.publish(odom_msg); @@ -532,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)))); + mapNameFrame("odom", r, static_cast(robotmodel->positionmodel)), + mapNameFrame("base_footprint", r, static_cast(robotmodel->positionmodel)))); // Also publish the ground truth pose and velocity Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose(); @@ -569,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 = mapNameFrame("odom", r, static_cast(robotmodel->positionmodel)); ground_truth_msg.header.stamp = sim_time; robotmodel->ground_truth_pub.publish(ground_truth_msg); @@ -606,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 = mapNameFrame("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 = mapNameFrame("camera", r,static_cast(robotmodel->positionmodel)); image_msg.header.stamp = sim_time; robotmodel->image_pubs[s].publish(image_msg); @@ -664,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 = mapNameFrame("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 = mapNameFrame("camera", r, static_cast(robotmodel->positionmodel)); depth_msg.header.stamp = sim_time; robotmodel->depth_pubs[s].publish(depth_msg); } @@ -687,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)))); + mapNameFrame("base_link", r, static_cast(robotmodel->positionmodel)), + mapNameFrame("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)))); + mapNameFrame("base_link", r, static_cast(robotmodel->positionmodel)), + mapNameFrame("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 = mapNameFrame("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 = mapNameFrame("camera", r, static_cast(robotmodel->positionmodel)); camera_msg.header.stamp = sim_time; camera_msg.height = cameramodel->getHeight(); camera_msg.width = cameramodel->getWidth();