From f4262adef6f3c483508659a45ceb9e175211035b Mon Sep 17 00:00:00 2001 From: Pablo Date: Sat, 10 Sep 2016 01:22:19 +0200 Subject: [PATCH 1/2] 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 b6a5869..a55a644 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/2] 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 a55a644..ef14284 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());