From f4262adef6f3c483508659a45ceb9e175211035b Mon Sep 17 00:00:00 2001 From: Pablo Date: Sat, 10 Sep 2016 01:22:19 +0200 Subject: [PATCH 1/5] ability to change tf frame names through rosparams --- src/stageros.cpp | 48 ++++++++++++++++++++++++++++++++++-------------- 1 file changed, 34 insertions(+), 14 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index b6a58699..a55a644c 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -132,6 +132,14 @@ class StageNode ros::Time base_last_cmd; ros::Duration base_watchdog_timeout; + std::string odom_frame_id_; + + std::string base_frame_id_; + + std::string base_frame_footprint_id_; + + std::string base_laser_frame_id_; + // Current simulation time ros::Time sim_time; @@ -277,6 +285,18 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us t = 0.2; this->base_watchdog_timeout.fromSec(t); + if(!localn.getParam("odom_frame_id", this->odom_frame_id_)) + this->odom_frame_id_="odom"; + + if(!localn.getParam("base_frame_id", this->base_frame_id_)) + this->base_frame_id_="base_link"; + + if(!localn.getParam("base_frame_footprint_id", this->base_frame_footprint_id_)) + this->base_frame_footprint_id_="base_footprint_link"; + + if(!localn.getParam("base_laser_frame_id", this->base_laser_frame_id_)) + this->base_laser_frame_id_="base_laser_link"; + if(!localn.getParam("is_depth_canonical", isDepthCanonical)) isDepthCanonical = true; @@ -461,9 +481,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 = mapName(this->base_laser_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); else - msg.header.frame_id = mapName("base_laser_link", r, static_cast(robotmodel->positionmodel)); + msg.header.frame_id = mapName(this->base_laser_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); msg.header.stamp = sim_time; robotmodel->laser_pubs[s].publish(msg); @@ -478,19 +498,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)))); + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->base_laser_frame_id_.c_str(), 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)))); + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->base_laser_frame_id_.c_str(), 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)))); + mapName( this->base_frame_footprint_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)))); // Get latest odometry data // Translate into ROS message format and publish @@ -506,7 +526,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 = mapName(this->odom_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); odom_msg.header.stamp = sim_time; robotmodel->odom_pub.publish(odom_msg); @@ -516,8 +536,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)))); + mapName(this->odom_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->base_frame_footprint_id_.c_str(), r, static_cast(robotmodel->positionmodel)))); // Also publish the ground truth pose and velocity Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose(); @@ -553,7 +573,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 = mapName(this->odom_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); ground_truth_msg.header.stamp = sim_time; robotmodel->ground_truth_pub.publish(ground_truth_msg); @@ -671,11 +691,11 @@ StageNode::WorldCallback() if (robotmodel->cameramodels.size() > 1) tf.sendTransform(tf::StampedTransform(tr, sim_time, - mapName("base_link", r, static_cast(robotmodel->positionmodel)), + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), mapName("camera", r, s, static_cast(robotmodel->positionmodel)))); else tf.sendTransform(tf::StampedTransform(tr, sim_time, - mapName("base_link", r, static_cast(robotmodel->positionmodel)), + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), mapName("camera", r, static_cast(robotmodel->positionmodel)))); sensor_msgs::CameraInfo camera_msg; From 69ec996f733f7f1ba6b750390d4c1dbf4cd38539 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pablo=20I=C3=B1igo=20Blasco?= Date: Sun, 18 Sep 2016 10:15:48 +0200 Subject: [PATCH 2/5] maximum range issue --- src/stageros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index a55a644c..ef14284d 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -470,7 +470,7 @@ StageNode::WorldCallback() msg.angle_max = +sensor.fov/2.0; msg.angle_increment = sensor.fov/(double)(sensor.sample_count-1); msg.range_min = sensor.range.min; - msg.range_max = sensor.range.max; + msg.range_max = sensor.range.max - 0.1; msg.ranges.resize(sensor.ranges.size()); msg.intensities.resize(sensor.intensities.size()); From 6aa91e4adc552a8c494f529d3af0a81ed8f5d97f Mon Sep 17 00:00:00 2001 From: doisyg Date: Thu, 26 Dec 2019 16:56:07 +0100 Subject: [PATCH 3/5] add camera frame id as param --- src/stageros.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 1eb80d48..74def88a 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -141,6 +141,8 @@ class StageNode std::string base_laser_frame_id_; + std::string base_camera_frame_id_; + // Current simulation time ros::Time sim_time; @@ -302,6 +304,9 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us if(!localn.getParam("base_laser_frame_id", this->base_laser_frame_id_)) this->base_laser_frame_id_="base_laser_link"; + if(!localn.getParam("base_camera_frame_id", this->base_camera_frame_id_)) + this->base_camera_frame_id_="camera"; + if(!localn.getParam("is_depth_canonical", isDepthCanonical)) isDepthCanonical = true; @@ -625,9 +630,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 = mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); else - image_msg.header.frame_id = mapName("camera", r,static_cast(robotmodel->positionmodel)); + image_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r,static_cast(robotmodel->positionmodel)); image_msg.header.stamp = sim_time; robotmodel->image_pubs[s].publish(image_msg); @@ -683,9 +688,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 = mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); else - depth_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel)); + depth_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); depth_msg.header.stamp = sim_time; robotmodel->depth_pubs[s].publish(depth_msg); } @@ -707,17 +712,17 @@ StageNode::WorldCallback() if (robotmodel->cameramodels.size() > 1) tf.sendTransform(tf::StampedTransform(tr, sim_time, mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName("camera", r, s, static_cast(robotmodel->positionmodel)))); + mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)))); else tf.sendTransform(tf::StampedTransform(tr, sim_time, mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName("camera", r, static_cast(robotmodel->positionmodel)))); + mapName(this->base_camera_frame_id_.c_str(), 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 = mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); else - camera_msg.header.frame_id = mapName("camera", r, static_cast(robotmodel->positionmodel)); + camera_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); camera_msg.header.stamp = sim_time; camera_msg.height = cameramodel->getHeight(); camera_msg.width = cameramodel->getWidth(); From 5ebed2255bf57bbbd17d318623ff65c190b3e8a4 Mon Sep 17 00:00:00 2001 From: doisyg Date: Thu, 26 Dec 2019 18:15:44 +0100 Subject: [PATCH 4/5] multi sensors frame_ids naming --- src/stageros.cpp | 98 ++++++++++++++++++++++++++++++++++-------------- 1 file changed, 69 insertions(+), 29 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 74def88a..816906ce 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -139,9 +139,9 @@ class StageNode std::string base_frame_footprint_id_; - std::string base_laser_frame_id_; + std::vector laser_frame_ids_; - std::string base_camera_frame_id_; + std::vector camera_frame_ids_; // Current simulation time ros::Time sim_time; @@ -295,17 +295,17 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us if(!localn.getParam("odom_frame_id", this->odom_frame_id_)) this->odom_frame_id_="odom"; - if(!localn.getParam("base_frame_id", this->base_frame_id_)) + if(!localn.getParam("base_link_frame_id", this->base_frame_id_)) this->base_frame_id_="base_link"; - if(!localn.getParam("base_frame_footprint_id", this->base_frame_footprint_id_)) - this->base_frame_footprint_id_="base_footprint_link"; + if(!localn.getParam("base_footprint_frame_id", this->base_frame_footprint_id_)) + this->base_frame_footprint_id_="base_footprint"; - if(!localn.getParam("base_laser_frame_id", this->base_laser_frame_id_)) - this->base_laser_frame_id_="base_laser_link"; + if(!localn.getParam("laser_frame_ids", this->laser_frame_ids_)) + this->laser_frame_ids_.push_back("base_laser"); - if(!localn.getParam("base_camera_frame_id", this->base_camera_frame_id_)) - this->base_camera_frame_id_="camera"; + if(!localn.getParam("camera_frame_ids", this->camera_frame_ids_)) + this->camera_frame_ids_.push_back("camera"); if(!localn.getParam("is_depth_canonical", isDepthCanonical)) isDepthCanonical = true; @@ -500,10 +500,16 @@ StageNode::WorldCallback() msg.intensities[i] = sensor.intensities[i]; } - if (robotmodel->lasermodels.size() > 1) - msg.header.frame_id = mapName(this->base_laser_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); + if (robotmodel->lasermodels.size() > 1){ + if (robotmodel->lasermodels.size() != this->laser_frame_ids_.size()){ + ROS_INFO("Size of laser_frame_ids mismatches the number of lasers, applying default frame id naming"); + msg.header.frame_id = mapName(this->laser_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)); + }else{ + msg.header.frame_id=this->laser_frame_ids_[s]; + } + } else - msg.header.frame_id = mapName(this->base_laser_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); + msg.header.frame_id = mapName(this->laser_frame_ids_[0].c_str(), r, static_cast(robotmodel->positionmodel)); msg.header.stamp = sim_time; robotmodel->laser_pubs[s].publish(msg); @@ -516,14 +522,22 @@ StageNode::WorldCallback() laserQ.setRPY(0.0, 0.0, lp.a); tf::Transform txLaser = tf::Transform(laserQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z)); - if (robotmodel->lasermodels.size() > 1) + if (robotmodel->lasermodels.size() > 1){ + if (robotmodel->lasermodels.size() != this->laser_frame_ids_.size()){ + ROS_INFO("Size of laser_frame_ids mismatches the number of lasers, applying default frame id naming"); + tf.sendTransform(tf::StampedTransform(txLaser, sim_time, + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->laser_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)))); + }else { tf.sendTransform(tf::StampedTransform(txLaser, sim_time, mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName(this->base_laser_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)))); + this->laser_frame_ids_[s])); + } + } else tf.sendTransform(tf::StampedTransform(txLaser, sim_time, mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName(this->base_laser_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)))); + mapName(this->laser_frame_ids_[0].c_str(), r, static_cast(robotmodel->positionmodel)))); } //the position of the robot @@ -629,10 +643,16 @@ StageNode::WorldCallback() memcpy(&(image_msg.data[(height-y)*linewidth]),temp,linewidth); } - if (robotmodel->cameramodels.size() > 1) - image_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); + if (robotmodel->cameramodels.size() > 1){ + if (robotmodel->cameramodels.size() != this->camera_frame_ids_.size()){ + ROS_INFO("Size of camera_frame_ids mismatches the number of cameras, applying default frame id naming"); + image_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)); + }else{ + image_msg.header.frame_id = this->camera_frame_ids_[s]; + } + } else - image_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r,static_cast(robotmodel->positionmodel)); + image_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r,static_cast(robotmodel->positionmodel)); image_msg.header.stamp = sim_time; robotmodel->image_pubs[s].publish(image_msg); @@ -687,10 +707,16 @@ StageNode::WorldCallback() memcpy(&(depth_msg.data[(height-y)*linewidth]),temp,linewidth); } - if (robotmodel->cameramodels.size() > 1) - depth_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); + if (robotmodel->cameramodels.size() > 1){ + if (robotmodel->cameramodels.size() != this->camera_frame_ids_.size()){ + ROS_INFO("Size of camera_frame_ids mismatches the number of cameras, applying default frame id naming"); + depth_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)); + }else{ + depth_msg.header.frame_id = this->camera_frame_ids_[s]; + } + } else - depth_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); + depth_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r,static_cast(robotmodel->positionmodel)); depth_msg.header.stamp = sim_time; robotmodel->depth_pubs[s].publish(depth_msg); } @@ -709,20 +735,34 @@ StageNode::WorldCallback() tf::Transform tr = tf::Transform(Q, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z+lp.z)); - if (robotmodel->cameramodels.size() > 1) - tf.sendTransform(tf::StampedTransform(tr, sim_time, - mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)))); + if (robotmodel->cameramodels.size() > 1){ + if (robotmodel->cameramodels.size() != this->camera_frame_ids_.size()){ + ROS_INFO("Size of camera_frame_ids mismatches the number of cameras, applying default frame id naming"); + tf.sendTransform(tf::StampedTransform(tr, sim_time, + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->camera_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)))); + }else { + tf.sendTransform(tf::StampedTransform(tr, sim_time, + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + this->camera_frame_ids_[s].c_str())); + } + } else tf.sendTransform(tf::StampedTransform(tr, sim_time, mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName(this->base_camera_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)))); + mapName(this->camera_frame_ids_[0].c_str(), r, static_cast(robotmodel->positionmodel)))); sensor_msgs::CameraInfo camera_msg; - if (robotmodel->cameramodels.size() > 1) - camera_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r, s, static_cast(robotmodel->positionmodel)); + if (robotmodel->cameramodels.size() > 1){ + if (robotmodel->cameramodels.size() != this->camera_frame_ids_.size()){ + ROS_INFO("Size of camera_frame_ids mismatches the number of cameras, applying default frame id naming"); + camera_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)); + }else { + camera_msg.header.frame_id = this->camera_frame_ids_[s]; + } + } else - camera_msg.header.frame_id = mapName(this->base_camera_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)); + camera_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r, static_cast(robotmodel->positionmodel)); camera_msg.header.stamp = sim_time; camera_msg.height = cameramodel->getHeight(); camera_msg.width = cameramodel->getWidth(); From 2379df82d94eceb985530142e26df29ac82d0fd0 Mon Sep 17 00:00:00 2001 From: doisyg Date: Thu, 26 Dec 2019 18:45:30 +0100 Subject: [PATCH 5/5] enable/disable tf per sensor --- src/stageros.cpp | 46 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 34 insertions(+), 12 deletions(-) diff --git a/src/stageros.cpp b/src/stageros.cpp index 816906ce..27b45f03 100644 --- a/src/stageros.cpp +++ b/src/stageros.cpp @@ -143,6 +143,10 @@ class StageNode std::vector camera_frame_ids_; + std::vector publish_laser_tfs_; + + std::vector publish_camera_tfs_; + // Current simulation time ros::Time sim_time; @@ -307,6 +311,12 @@ StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool us if(!localn.getParam("camera_frame_ids", this->camera_frame_ids_)) this->camera_frame_ids_.push_back("camera"); + if(!localn.getParam("publish_laser_tfs", this->publish_laser_tfs_)) + this->publish_laser_tfs_.push_back(true); + + if(!localn.getParam("publish_camera_tfs", this->publish_camera_tfs_)) + this->publish_camera_tfs_.push_back(true); + if(!localn.getParam("is_depth_canonical", isDepthCanonical)) isDepthCanonical = true; @@ -525,19 +535,25 @@ StageNode::WorldCallback() if (robotmodel->lasermodels.size() > 1){ if (robotmodel->lasermodels.size() != this->laser_frame_ids_.size()){ ROS_INFO("Size of laser_frame_ids mismatches the number of lasers, applying default frame id naming"); - tf.sendTransform(tf::StampedTransform(txLaser, sim_time, - mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName(this->laser_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)))); + if(publish_laser_tfs_[0]){ + tf.sendTransform(tf::StampedTransform(txLaser, sim_time, + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->laser_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)))); + } }else { - tf.sendTransform(tf::StampedTransform(txLaser, sim_time, - mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - this->laser_frame_ids_[s])); + if(publish_laser_tfs_[s]){ + tf.sendTransform(tf::StampedTransform(txLaser, sim_time, + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + this->laser_frame_ids_[s])); + } } } else + if(publish_laser_tfs_[0]){ tf.sendTransform(tf::StampedTransform(txLaser, sim_time, mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), mapName(this->laser_frame_ids_[0].c_str(), r, static_cast(robotmodel->positionmodel)))); + } } //the position of the robot @@ -738,19 +754,25 @@ StageNode::WorldCallback() if (robotmodel->cameramodels.size() > 1){ if (robotmodel->cameramodels.size() != this->camera_frame_ids_.size()){ ROS_INFO("Size of camera_frame_ids mismatches the number of cameras, applying default frame id naming"); - tf.sendTransform(tf::StampedTransform(tr, sim_time, - mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - mapName(this->camera_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)))); + if(publish_camera_tfs_[0]){ + tf.sendTransform(tf::StampedTransform(tr, sim_time, + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + mapName(this->camera_frame_ids_[0].c_str(), r, s, static_cast(robotmodel->positionmodel)))); + } }else { - tf.sendTransform(tf::StampedTransform(tr, sim_time, - mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), - this->camera_frame_ids_[s].c_str())); + if(publish_camera_tfs_[s]){ + tf.sendTransform(tf::StampedTransform(tr, sim_time, + mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), + this->camera_frame_ids_[s].c_str())); + } } } else + if(publish_camera_tfs_[0]){ tf.sendTransform(tf::StampedTransform(tr, sim_time, mapName(this->base_frame_id_.c_str(), r, static_cast(robotmodel->positionmodel)), mapName(this->camera_frame_ids_[0].c_str(), r, static_cast(robotmodel->positionmodel)))); + } sensor_msgs::CameraInfo camera_msg; if (robotmodel->cameramodels.size() > 1){