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

Multi-sensors (laser and camera) frame_id renaming and TF publish control #58

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

Multi-sensors (laser and camera) frame_id renaming and TF publish control #58

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f4262ad
ability to change tf frame names through rosparams
pabloinigoblasco Sep 9, 2016
69ec996
maximum range issue
pabloinigoblasco Sep 18, 2016
208e2a3
Merge pull request #1 from pabloinigoblasco/master
doisyg Dec 26, 2019
6aa91e4
add camera frame id as param
doisyg Dec 26, 2019
5ebed22
multi sensors frame_ids naming
doisyg Dec 26, 2019
2379df8
enable/disable tf per sensor
doisyg Dec 26, 2019
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
149 changes: 118 additions & 31 deletions src/stageros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,20 @@ 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::vector<std::string> laser_frame_ids_;

std::vector<std::string> camera_frame_ids_;

std::vector<bool> publish_laser_tfs_;

std::vector<bool> publish_camera_tfs_;

// Current simulation time
ros::Time sim_time;

Expand Down Expand Up @@ -282,6 +296,27 @@ 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_link_frame_id", this->base_frame_id_))
this->base_frame_id_="base_link";

if(!localn.getParam("base_footprint_frame_id", this->base_frame_footprint_id_))
this->base_frame_footprint_id_="base_footprint";

if(!localn.getParam("laser_frame_ids", this->laser_frame_ids_))
this->laser_frame_ids_.push_back("base_laser");

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;

Expand Down Expand Up @@ -465,7 +500,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());

Expand All @@ -475,10 +510,16 @@ StageNode::WorldCallback()
msg.intensities[i] = sensor.intensities[i];
}

if (robotmodel->lasermodels.size() > 1)
msg.header.frame_id = mapName("base_laser_link", r, s, static_cast<Stg::Model*>(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<Stg::Model*>(robotmodel->positionmodel));
}else{
msg.header.frame_id=this->laser_frame_ids_[s];
}
}
else
msg.header.frame_id = mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
msg.header.frame_id = mapName(this->laser_frame_ids_[0].c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel));

msg.header.stamp = sim_time;
robotmodel->laser_pubs[s].publish(msg);
Expand All @@ -491,21 +532,35 @@ 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)
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))));
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");
if(publish_laser_tfs_[0]){
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
mapName(this->base_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName(this->laser_frame_ids_[0].c_str(), r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
}
}else {
if(publish_laser_tfs_[s]){
tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
mapName(this->base_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
this->laser_frame_ids_[s]));
}
}
}
else
if(publish_laser_tfs_[0]){
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))));
mapName(this->base_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName(this->laser_frame_ids_[0].c_str(), 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))));
mapName( this->base_frame_footprint_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName(this->base_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel))));

// Get latest odometry data
// Translate into ROS message format and publish
Expand All @@ -521,7 +576,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 = mapName(this->odom_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel));
odom_msg.header.stamp = sim_time;

robotmodel->odom_pub.publish(odom_msg);
Expand All @@ -531,8 +586,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))));
mapName(this->odom_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName(this->base_frame_footprint_id_.c_str(), 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 +623,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 = mapName(this->odom_frame_id_.c_str(), 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 @@ -604,10 +659,16 @@ StageNode::WorldCallback()
memcpy(&(image_msg.data[(height-y)*linewidth]),temp,linewidth);
}

if (robotmodel->cameramodels.size() > 1)
image_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(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<Stg::Model*>(robotmodel->positionmodel));
}else{
image_msg.header.frame_id = this->camera_frame_ids_[s];
}
}
else
image_msg.header.frame_id = mapName("camera", r,static_cast<Stg::Model*>(robotmodel->positionmodel));
image_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r,static_cast<Stg::Model*>(robotmodel->positionmodel));
image_msg.header.stamp = sim_time;

robotmodel->image_pubs[s].publish(image_msg);
Expand Down Expand Up @@ -662,10 +723,16 @@ StageNode::WorldCallback()
memcpy(&(depth_msg.data[(height-y)*linewidth]),temp,linewidth);
}

if (robotmodel->cameramodels.size() > 1)
depth_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(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<Stg::Model*>(robotmodel->positionmodel));
}else{
depth_msg.header.frame_id = this->camera_frame_ids_[s];
}
}
else
depth_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
depth_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), r,static_cast<Stg::Model*>(robotmodel->positionmodel));
depth_msg.header.stamp = sim_time;
robotmodel->depth_pubs[s].publish(depth_msg);
}
Expand All @@ -684,20 +751,40 @@ 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("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName("camera", r, s, static_cast<Stg::Model*>(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");
if(publish_camera_tfs_[0]){
tf.sendTransform(tf::StampedTransform(tr, sim_time,
mapName(this->base_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName(this->camera_frame_ids_[0].c_str(), r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
}
}else {
if(publish_camera_tfs_[s]){
tf.sendTransform(tf::StampedTransform(tr, sim_time,
mapName(this->base_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
this->camera_frame_ids_[s].c_str()));
}
}
}
else
if(publish_camera_tfs_[0]){
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))));
mapName(this->base_frame_id_.c_str(), r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
mapName(this->camera_frame_ids_[0].c_str(), 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));
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<Stg::Model*>(robotmodel->positionmodel));
}else {
camera_msg.header.frame_id = this->camera_frame_ids_[s];
}
}
else
camera_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
camera_msg.header.frame_id = mapName(this->camera_frame_ids_[0].c_str(), 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