diff --git a/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp b/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp index 5d4c458d..8dae3b9f 100644 --- a/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp +++ b/slam_toolbox/include/slam_toolbox/experimental/slam_toolbox_lifelong.hpp @@ -42,7 +42,7 @@ class LifelongSlamToolbox : public SlamToolbox protected: virtual void laserCallback( - const sensor_msgs::LaserScan::ConstPtr& scan) override final; + const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final; virtual bool deserializePoseGraphCallback( slam_toolbox_msgs::DeserializePoseGraph::Request& req, slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final; diff --git a/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp b/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp index b65f5496..96353c3d 100644 --- a/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp +++ b/slam_toolbox/include/slam_toolbox/slam_toolbox_async.hpp @@ -33,7 +33,7 @@ class AsynchronousSlamToolbox : public SlamToolbox protected: virtual void laserCallback( - const sensor_msgs::LaserScan::ConstPtr& scan) override final; + const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final; virtual bool deserializePoseGraphCallback( slam_toolbox_msgs::DeserializePoseGraph::Request& req, slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final; diff --git a/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp b/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp index bd8d9edd..1434a4ad 100644 --- a/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp +++ b/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp @@ -70,7 +70,7 @@ class SlamToolbox void setROSInterfaces(ros::NodeHandle& node); // callbacks - virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) = 0; + virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) = 0; bool mapCallback(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res); virtual bool serializePoseGraphCallback(slam_toolbox_msgs::SerializePoseGraph::Request& req, @@ -123,7 +123,7 @@ class SlamToolbox std::unique_ptr dataset_; std::map lasers_; std::map m_map_to_odoms_; - std::map m_base_id_to_odom_id_; + std::map m_base_id_to_odom_id_, m_laser_id_to_base_id_; // helpers std::map> laser_assistants_; @@ -134,7 +134,7 @@ class SlamToolbox // Internal state std::vector > threads_; - boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_; + boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_, laser_id_map_mutex_; PausedState state_; nav_msgs::GetMap::Response map_; ProcessType processor_type_; diff --git a/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp b/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp index 594fcedd..a9533b1f 100644 --- a/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp +++ b/slam_toolbox/include/slam_toolbox/slam_toolbox_localization.hpp @@ -35,7 +35,7 @@ class LocalizationSlamToolbox : public SlamToolbox protected: virtual void laserCallback( - const sensor_msgs::LaserScan::ConstPtr& scan) override final; + const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final; void localizePoseCallback( const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); diff --git a/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp b/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp index 542fbdef..ab8d3f35 100644 --- a/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp +++ b/slam_toolbox/include/slam_toolbox/slam_toolbox_sync.hpp @@ -33,7 +33,7 @@ class SynchronousSlamToolbox : public SlamToolbox void run(); protected: - virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) override final; + virtual void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) override final; bool clearQueueCallback(slam_toolbox_msgs::ClearQueue::Request& req, slam_toolbox_msgs::ClearQueue::Response& resp); virtual bool deserializePoseGraphCallback(slam_toolbox_msgs::DeserializePoseGraph::Request& req, slam_toolbox_msgs::DeserializePoseGraph::Response& resp) override final; diff --git a/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp b/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp index f5af2610..740dee0b 100644 --- a/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp +++ b/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp @@ -66,7 +66,7 @@ LifelongSlamToolbox::LifelongSlamToolbox(ros::NodeHandle& nh) /*****************************************************************************/ void LifelongSlamToolbox::laserCallback( - const sensor_msgs::LaserScan::ConstPtr& scan) + const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) /*****************************************************************************/ { // no odom info on any pose helper diff --git a/slam_toolbox/src/slam_toolbox_async.cpp b/slam_toolbox/src/slam_toolbox_async.cpp index 3cf6f35e..8e48a734 100644 --- a/slam_toolbox/src/slam_toolbox_async.cpp +++ b/slam_toolbox/src/slam_toolbox_async.cpp @@ -32,7 +32,7 @@ AsynchronousSlamToolbox::AsynchronousSlamToolbox(ros::NodeHandle& nh) /*****************************************************************************/ void AsynchronousSlamToolbox::laserCallback( - const sensor_msgs::LaserScan::ConstPtr& scan) + const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) /*****************************************************************************/ { // no odom info on any pose helper @@ -40,13 +40,23 @@ void AsynchronousSlamToolbox::laserCallback( bool found_odom = false; for(size_t idx = 0; idx < pose_helpers_.size(); idx++) { - found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id); + // try compute odometry + found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id); if(found_odom) break; } if(!found_odom) return; + // Add new sensor to laser ID map, and to laser assistants + { + boost::mutex::scoped_lock l(laser_id_map_mutex_); + if(m_laser_id_to_base_id_.find(scan->header.frame_id) == m_laser_id_to_base_id_.end()) + { + m_laser_id_to_base_id_[scan->header.frame_id] = base_frame_id; + laser_assistants_[scan->header.frame_id] = std::make_unique(nh_, tf_.get(), base_frame_id); + } + } // ensure the laser can be used karto::LaserRangeFinder* laser = getLaser(scan); diff --git a/slam_toolbox/src/slam_toolbox_common.cpp b/slam_toolbox/src/slam_toolbox_common.cpp index f349628b..bbc1640f 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -46,16 +46,15 @@ SlamToolbox::SlamToolbox(ros::NodeHandle& nh) ROS_FATAL("[RoboSAR:slam_toolbox_common:SlamToolbox] base_frames_.size() != odom_frames_.size()"); assert(base_frames_.size() == laser_topics_.size()); assert(base_frames_.size() == odom_frames_.size()); - // Create map to compute odom frame given base frame + // Setup map: base frame to odom frame for(size_t idx = 0; idx < base_frames_.size(); idx++) { m_base_id_to_odom_id_[base_frames_[idx]] = odom_frames_[idx]; } - // Set up pose helpers and laser assistants for each robot + // Set up pose helpers for each robot for(size_t idx = 0; idx < base_frames_.size(); idx++) { pose_helpers_.push_back(std::make_unique(tf_.get(), base_frames_[idx], odom_frames_[idx])); - laser_assistants_[base_frames_[idx]] = std::make_unique(nh_, tf_.get(), base_frames_[idx]); // Assumes base frame = laser frame } scan_holder_ = std::make_unique(lasers_); map_saver_ = std::make_unique(nh_, map_name_); @@ -90,7 +89,6 @@ SlamToolbox::~SlamToolbox() for(size_t idx = 0; idx < pose_helpers_.size(); idx++) { pose_helpers_[idx].reset(); - // laser_assistants_.reset(); } for(std::map>::iterator it = laser_assistants_.begin(); it != laser_assistants_.end(); it++) { @@ -189,7 +187,7 @@ void SlamToolbox::setROSInterfaces(ros::NodeHandle& node) ROS_INFO("Subscribing to scan: %s", laser_topics_[idx].c_str()); scan_filter_subs_.push_back(std::make_unique >(node, laser_topics_[idx], 5)); scan_filters_.push_back(std::make_unique >(*scan_filter_subs_.back(), *tf_, odom_frames_[idx], 5, node)); - scan_filters_.back()->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1)); + scan_filters_.back()->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1, base_frames_[idx])); } } @@ -331,12 +329,19 @@ karto::LaserRangeFinder* SlamToolbox::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan) /*****************************************************************************/ { + // Use laser scan ID to get base frame ID const std::string& frame = scan->header.frame_id; if(lasers_.find(frame) == lasers_.end()) { try { - lasers_[frame] = laser_assistants_[frame]->toLaserMetadata(*scan); + std::map>::const_iterator it = laser_assistants_.find(frame); + if(it == laser_assistants_.end()) + { + ROS_ERROR("Failed to get requested laser assistant, aborting initialization (%s)", frame.c_str()); + return nullptr; + } + lasers_[frame] = it->second->toLaserMetadata(*scan); dataset_->Add(lasers_[frame].getLaser(), true); } catch (tf2::TransformException& e) @@ -385,16 +390,27 @@ tf2::Stamped SlamToolbox::setTransformFromPoses( const bool& update_reprocessing_transform) /*****************************************************************************/ { - // Use base frame to look up odom frame - std::string odom_frame = m_base_id_to_odom_id_[header.frame_id]; + tf2::Stamped odom_to_map; + // Use laser frame to look up base and odom frame + std::string base_frame; + { + boost::mutex::scoped_lock l(laser_id_map_mutex_); + std::map::const_iterator it = m_laser_id_to_base_id_.find(header.frame_id); + if(it == m_laser_id_to_base_id_.end()) + { + ROS_ERROR("Requested laser frame ID not in map: %s", header.frame_id.c_str()); + return odom_to_map; + } + base_frame = it->second; + } + const std::string& odom_frame = m_base_id_to_odom_id_[base_frame]; // Compute the map->odom transform const ros::Time& t = header.stamp; - tf2::Stamped odom_to_map; tf2::Quaternion q(0.,0.,0.,1.0); q.setRPY(0., 0., corrected_pose.GetHeading()); tf2::Stamped base_to_map( tf2::Transform(q, tf2::Vector3(corrected_pose.GetX(), - corrected_pose.GetY(), 0.0)).inverse(), t, header.frame_id); // Assumes base frame = laser frame + corrected_pose.GetY(), 0.0)).inverse(), t, base_frame); try { geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg; @@ -735,8 +751,14 @@ void SlamToolbox::loadSerializedPoseGraph( ROS_INFO("Got scan!"); try { - lasers_[scan->header.frame_id] = - laser_assistants_[scan->header.frame_id]->toLaserMetadata(*scan); + const std::string& frame = scan->header.frame_id; + std::map>::const_iterator it = laser_assistants_.find(frame); + if(it == laser_assistants_.end()) + { + ROS_ERROR("Failed to get requested laser assistant, aborting continue mapping (%s)", frame.c_str()); + exit(-1); + } + lasers_[frame] = it->second->toLaserMetadata(*scan); break; } catch (tf2::TransformException& e) diff --git a/slam_toolbox/src/slam_toolbox_localization.cpp b/slam_toolbox/src/slam_toolbox_localization.cpp index 05a08727..425dbf07 100644 --- a/slam_toolbox/src/slam_toolbox_localization.cpp +++ b/slam_toolbox/src/slam_toolbox_localization.cpp @@ -101,7 +101,7 @@ bool LocalizationSlamToolbox::deserializePoseGraphCallback( /*****************************************************************************/ void LocalizationSlamToolbox::laserCallback( - const sensor_msgs::LaserScan::ConstPtr& scan) + const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) /*****************************************************************************/ { // no odom info on any pose helper diff --git a/slam_toolbox/src/slam_toolbox_sync.cpp b/slam_toolbox/src/slam_toolbox_sync.cpp index f28820ad..357c5217 100644 --- a/slam_toolbox/src/slam_toolbox_sync.cpp +++ b/slam_toolbox/src/slam_toolbox_sync.cpp @@ -65,7 +65,7 @@ void SynchronousSlamToolbox::run() /*****************************************************************************/ void SynchronousSlamToolbox::laserCallback( - const sensor_msgs::LaserScan::ConstPtr& scan) + const sensor_msgs::LaserScan::ConstPtr& scan, const std::string& base_frame_id) /*****************************************************************************/ { // no odom info on any pose helper @@ -73,13 +73,23 @@ void SynchronousSlamToolbox::laserCallback( bool found_odom = false; for(size_t idx = 0; idx < pose_helpers_.size(); idx++) { - found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id); + // try compute odometry + found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, base_frame_id); if(found_odom) break; } if(!found_odom) return; + // Add new sensor to laser ID map, and to laser assistants + { + boost::mutex::scoped_lock l(laser_id_map_mutex_); + if(m_laser_id_to_base_id_.find(scan->header.frame_id) == m_laser_id_to_base_id_.end()) + { + m_laser_id_to_base_id_[scan->header.frame_id] = base_frame_id; + laser_assistants_[scan->header.frame_id] = std::make_unique(nh_, tf_.get(), base_frame_id); + } + } // ensure the laser can be used karto::LaserRangeFinder* laser = getLaser(scan);