From d9f2bcd7e4ac1cea8576874261732d3e26d64f5e Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Tue, 3 Feb 2015 00:55:50 +0100 Subject: [PATCH] get code to work with frames starting with "/" This is to get test_rosie_multilaser.xml to work --- amcl/src/amcl_node.cpp | 44 +++++++++++++++++++++++++++++------------- 1 file changed, 31 insertions(+), 13 deletions(-) diff --git a/amcl/src/amcl_node.cpp b/amcl/src/amcl_node.cpp index 1fd569e8b..514567545 100644 --- a/amcl/src/amcl_node.cpp +++ b/amcl/src/amcl_node.cpp @@ -107,6 +107,18 @@ angle_diff(double a, double b) static const std::string scan_topic_ = "scan"; +/* This function is only useful to have the whole code work + * with old rosbags that have trailing slashes for their frames + */ +inline +std::string stripSlash(const std::string& in) +{ + std::string out = in; + if ( ( !in.empty() ) && (in[0] == '/') ) + out.erase(0,1); + return out; +} + class AmclNode { public: @@ -383,6 +395,10 @@ AmclNode::AmclNode() : transform_tolerance_.fromSec(tmp_tol); + odom_frame_id_ = stripSlash(odom_frame_id_); + base_frame_id_ = stripSlash(base_frame_id_); + global_frame_id_ = stripSlash(global_frame_id_); + updatePoseFromServer(); cloud_pub_interval.fromSec(1.0); @@ -555,9 +571,9 @@ void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level) ROS_INFO("Done initializing likelihood field model."); } - odom_frame_id_ = config.odom_frame_id; - base_frame_id_ = config.base_frame_id; - global_frame_id_ = config.global_frame_id; + odom_frame_id_ = stripSlash(config.odom_frame_id); + base_frame_id_ = stripSlash(config.base_frame_id); + global_frame_id_ = stripSlash(config.global_frame_id); delete laser_scan_filter_; laser_scan_filter_ = new tf2_ros::MessageFilter(*laser_scan_sub_, @@ -826,7 +842,7 @@ AmclNode::getOdomPose(geometry_msgs::PoseStamped& odom_pose, { // Get the robot's pose geometry_msgs::PoseStamped ident; - ident.header.frame_id = f; + ident.header.frame_id = stripSlash(f); ident.header.stamp = t; tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); try @@ -923,6 +939,7 @@ AmclNode::setMapCallback(nav_msgs::SetMap::Request& req, void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) { + std::string laser_scan_frame_id = stripSlash(laser_scan->header.frame_id); last_laser_received_ts_ = ros::Time::now(); if( map_ == NULL ) { return; @@ -931,15 +948,15 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) int laser_index = -1; // Do we have the base->base_laser Tx yet? - if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end()) + if(frame_to_laser_.find(laser_scan_frame_id) == frame_to_laser_.end()) { - ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str()); + ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan_frame_id.c_str()); lasers_.push_back(new AMCLLaser(*laser_)); lasers_update_.push_back(true); laser_index = frame_to_laser_.size(); geometry_msgs::PoseStamped ident; - ident.header.frame_id = laser_scan->header.frame_id; + ident.header.frame_id = stripSlash(laser_scan_frame_id); ident.header.stamp = ros::Time(); tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); @@ -952,7 +969,7 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) { ROS_ERROR("Couldn't transform from %s to %s, " "even though the message notifier is in use", - laser_scan->header.frame_id.c_str(), + laser_scan_frame_id.c_str(), base_frame_id_.c_str()); return; } @@ -968,10 +985,10 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) laser_pose_v.v[1], laser_pose_v.v[2]); - frame_to_laser_[laser_scan->header.frame_id] = laser_index; + frame_to_laser_[laser_scan_frame_id] = laser_index; } else { // we have the laser pose, retrieve laser index - laser_index = frame_to_laser_[laser_scan->header.frame_id]; + laser_index = frame_to_laser_[laser_scan_frame_id]; } // Where was the robot when this scan was taken? @@ -1059,7 +1076,8 @@ AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan) tf2::Quaternion q; q.setRPY(0.0, 0.0, laser_scan->angle_min); geometry_msgs::QuaternionStamped min_q, inc_q; - min_q.header = laser_scan->header; + min_q.header.stamp = laser_scan->header.stamp; + min_q.header.frame_id = stripSlash(laser_scan->header.frame_id); tf2::convert(q, min_q.quaternion); q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment); @@ -1326,10 +1344,10 @@ AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStampe ROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id."); } // We only accept initial pose estimates in the global frame, #5148. - else if(msg.header.frame_id != global_frame_id_) + else if(stripSlash(msg.header.frame_id) != global_frame_id_) { ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"", - msg.header.frame_id.c_str(), + stripSlash(msg.header.frame_id).c_str(), global_frame_id_.c_str()); return; }