diff --git a/README.md b/README.md index aa1ba876..ca9c0313 100644 --- a/README.md +++ b/README.md @@ -46,6 +46,17 @@ The video below was collected at [Circuit Launch](https://www.circuitlaunch.com/ ![map_image](/images/circuit_launch.gif?raw=true "Map Image") +# Multi-Robot SLAM +SLAM Toolbox supports multi-robot SLAM. The robots must all start in the same location, facing the same way. The first robot to connect determines the world origin, and the relative starting points of the other robots are determined using laser scan matching. This allows all robots to add to a shared pose graph, which makes possible: +- Localization of multiple robots +- Mapping using information from all sensor readings +- Loop closures across robots + +Currently, synchornous and asynchornous mapping is supported. + +![MRSLAM_gif](/images/mrslam_readme.gif?raw=true "Multi-Robot SLAM") + +`odom_frames`, `base_frames`, and `laser_topics` define odom frame, base frame, and laser topic for all robots. These params must all have the same order of robots, and must have the same number of elements. This means that each robot can only have one LIDAR. # 03/23/2021 Note On Serialized Files diff --git a/images/mrslam_readme.gif b/images/mrslam_readme.gif new file mode 100644 index 00000000..cea8846c Binary files /dev/null and b/images/mrslam_readme.gif differ diff --git a/slam_toolbox/config/mapper_params_online_async.yaml b/slam_toolbox/config/mapper_params_online_async.yaml index f977870c..6b5869a3 100644 --- a/slam_toolbox/config/mapper_params_online_async.yaml +++ b/slam_toolbox/config/mapper_params_online_async.yaml @@ -7,17 +7,17 @@ ceres_dogleg_type: TRADITIONAL_DOGLEG ceres_loss_function: None # ROS Parameters -odom_frame: odom map_frame: map -base_frame: base_footprint -scan_topic: /scan +odom_frames: ["robot1/odom", "robot2/odom"] +base_frames: ["robot1/base_footprint", "robot2/base_footprint"] +laser_topics: ["/robot1/scan", "/robot2/scan"] mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose # or at the dock, but they are mutually exclusive, if pose is given # will use pose #map_file_name: test_steve -# map_start_pose: [0.0, 0.0, 0.0] +#map_start_pose: [0.0, 0.0, 0.0] #map_start_at_dock: true debug_logging: false diff --git a/slam_toolbox/config/mapper_params_online_sync.yaml b/slam_toolbox/config/mapper_params_online_sync.yaml index ae457852..6b5869a3 100644 --- a/slam_toolbox/config/mapper_params_online_sync.yaml +++ b/slam_toolbox/config/mapper_params_online_sync.yaml @@ -7,10 +7,10 @@ ceres_dogleg_type: TRADITIONAL_DOGLEG ceres_loss_function: None # ROS Parameters -odom_frame: odom map_frame: map -base_frame: base_footprint -scan_topic: /scan +odom_frames: ["robot1/odom", "robot2/odom"] +base_frames: ["robot1/base_footprint", "robot2/base_footprint"] +laser_topics: ["/robot1/scan", "/robot2/scan"] mode: mapping #localization # if you'd like to immediately start continuing a map at a given pose 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/get_pose_helper.hpp b/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp index 5a670532..4b130b4b 100644 --- a/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp +++ b/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp @@ -37,8 +37,13 @@ class GetPoseHelper { }; - bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t) + bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t, std::string ref_frame) { + // Only succeed if ref_frame matches base_frame_; otherwise using wrong pose helper + if(ref_frame != base_frame_) + { + return false; + } geometry_msgs::TransformStamped base_ident, odom_pose; base_ident.header.stamp = t; base_ident.header.frame_id = base_frame_; diff --git a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp index 98611d82..4c64fc76 100644 --- a/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp +++ b/slam_toolbox/include/slam_toolbox/loop_closure_assistant.hpp @@ -55,6 +55,7 @@ class LoopClosureAssistant bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp); void moveNode(const int& id, const Eigen::Vector3d& pose); void addMovedNodes(const int& id, Eigen::Vector3d vec); + std_msgs::ColorRGBA createNewColor(); std::unique_ptr tfB_; laser_utils::ScanHolder* scan_holder_; @@ -72,6 +73,7 @@ class LoopClosureAssistant std::string map_frame_; PausedState& state_; ProcessType& processor_type_; + std::map m_sensor_name_to_color_; }; } // end namespace 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 0c5e1e60..a54acbed 100644 --- a/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp +++ b/slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp @@ -45,6 +45,7 @@ #include #include #include +#include namespace slam_toolbox { @@ -69,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, @@ -86,7 +87,7 @@ class SlamToolbox karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, PosedScan& scanWPose); bool updateMap(); tf2::Stamped setTransformFromPoses(const karto::Pose2& pose, - const karto::Pose2& karto_pose, const ros::Time& t, const bool& update_reprocessing_transform); + const karto::Pose2& karto_pose, const std_msgs::Header& header, const bool& update_reprocessing_transform); karto::LocalizedRangeScan* getLocalizedRangeScan(karto::LaserRangeFinder* laser, const sensor_msgs::LaserScan::ConstPtr& scan, karto::Pose2& karto_pose); @@ -103,13 +104,14 @@ class SlamToolbox std::unique_ptr tf_; std::unique_ptr tfL_; std::unique_ptr tfB_; - std::unique_ptr > scan_filter_sub_; - std::unique_ptr > scan_filter_; + std::vector > > scan_filter_subs_; + std::vector > > scan_filters_; ros::Publisher sst_, sstm_; ros::ServiceServer ssMap_, ssPauseMeasurements_, ssSerialize_, ssDesserialize_; // Storage for ROS parameters - std::string odom_frame_, map_frame_, base_frame_, map_name_, scan_topic_; + std::string map_frame_, map_name_; + std::vector odom_frames_, base_frames_, laser_topics_; ros::Duration transform_timeout_, tf_buffer_dur_, minimum_time_interval_; int throttle_scans_; @@ -120,18 +122,19 @@ class SlamToolbox std::unique_ptr smapper_; std::unique_ptr dataset_; std::map lasers_; + std::map m_map_to_odoms_; + std::map m_base_id_to_odom_id_, m_laser_id_to_base_id_; // helpers - std::unique_ptr laser_assistant_; - std::unique_ptr pose_helper_; + std::map> laser_assistants_; + std::map> pose_helpers_; std::unique_ptr map_saver_; std::unique_ptr closure_assistant_; std::unique_ptr scan_holder_; // Internal state std::vector > threads_; - tf2::Transform map_to_odom_; - 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/lib/karto_sdk/src/Mapper.cpp b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp index 74662169..389802a4 100644 --- a/slam_toolbox/lib/karto_sdk/src/Mapper.cpp +++ b/slam_toolbox/lib/karto_sdk/src/Mapper.cpp @@ -1513,7 +1513,7 @@ namespace karto kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan(pScan, pSensorManager->GetScans(rCandidateSensorName), bestPose, covariance); - LinkScans(pScan, pSensorManager->GetScan(rCandidateSensorName, 0), bestPose, covariance); + LinkScans(pSensorManager->GetScan(rCandidateSensorName, 0), pScan, bestPose, covariance); // only add to means and covariances if response was high "enough" if (response > m_pMapper->m_pLinkMatchMinimumResponseFine->GetValue()) diff --git a/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp b/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp index cf17a721..9b3a53c8 100644 --- a/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp +++ b/slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp @@ -66,14 +66,16 @@ 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 - Pose2 pose; - if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) - { + // no odom info on any pose helper + karto::Pose2 pose; + if(pose_helpers_.find(base_frame_id) == pose_helpers_.end()) return; + else + { + pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id); } // ensure the laser can be used diff --git a/slam_toolbox/src/laser_utils.cpp b/slam_toolbox/src/laser_utils.cpp index b0325497..98110431 100644 --- a/slam_toolbox/src/laser_utils.cpp +++ b/slam_toolbox/src/laser_utils.cpp @@ -92,9 +92,10 @@ LaserMetadata LaserAssistant::toLaserMetadata(sensor_msgs::LaserScan scan) karto::LaserRangeFinder* LaserAssistant::makeLaser(const double& mountingYaw) { + const std::string name = scan_.header.frame_id; karto::LaserRangeFinder* laser = karto::LaserRangeFinder::CreateLaserRangeFinder( - karto::LaserRangeFinder_Custom, karto::Name("Custom Described Lidar")); + karto::LaserRangeFinder_Custom, karto::Name(name.c_str())); // Sets name to laser frame laser->SetOffsetPose(karto::Pose2(laser_pose_.transform.translation.x, laser_pose_.transform.translation.y, mountingYaw)); laser->SetMinimumRange(scan_.range_min); diff --git a/slam_toolbox/src/loop_closure_assistant.cpp b/slam_toolbox/src/loop_closure_assistant.cpp index 1098ef30..fc20c3a5 100644 --- a/slam_toolbox/src/loop_closure_assistant.cpp +++ b/slam_toolbox/src/loop_closure_assistant.cpp @@ -171,6 +171,12 @@ void LoopClosureAssistant::publishGraph() inner_it != outer_it->second.end(); ++inner_it) { karto::LocalizedRangeScan * scan = inner_it->second->GetObject(); + // Update color map and marker color + karto::Name cur_sensor = scan->GetSensorName(); + std::map::const_iterator map_it = m_sensor_name_to_color_.find(cur_sensor); + if(map_it == m_sensor_name_to_color_.end()) + m_sensor_name_to_color_[cur_sensor] = createNewColor(); + vertex_marker.color = m_sensor_name_to_color_[cur_sensor]; vertex_marker.pose.position.x = scan->GetCorrectedPose().GetX(); vertex_marker.pose.position.y = scan->GetCorrectedPose().GetY(); @@ -211,6 +217,20 @@ void LoopClosureAssistant::publishGraph() const karto::Edge * edge = *it; karto::LocalizedRangeScan * source_scan = edge->GetSource()->GetObject(); karto::LocalizedRangeScan * target_scan = edge->GetTarget()->GetObject(); + // Set edge color based on if edge is for one robot, or two + karto::Name source_sensor = source_scan->GetSensorName(); + if(source_sensor == target_scan->GetSensorName()) + { + edge_marker.color = m_sensor_name_to_color_[source_sensor]; + edge_marker.color.a = 0.5; + } + else + { + edge_marker.color.r = 1.0; + edge_marker.color.g = 0.0; + edge_marker.color.b = 0.0; + edge_marker.color.a = 0.5; + } const karto::Pose2 source_pose = source_scan->GetCorrectedPose(); const karto::Pose2 target_pose = target_scan->GetCorrectedPose(); @@ -362,4 +382,16 @@ void LoopClosureAssistant::addMovedNodes(const int& id, Eigen::Vector3d vec) moved_nodes_[id] = vec; } +/*****************************************************************************/ +std_msgs::ColorRGBA LoopClosureAssistant::createNewColor() +{ + std_msgs::ColorRGBA new_color; + new_color.r = (float)(rand() % 1000) / 1000; + new_color.g = (float)(rand() % 1000) / 1000; + new_color.b = (float)(rand() % 1000) / 1000; + new_color.a = 1; + return new_color; +} +/*****************************************************************************/ + } // end namespace diff --git a/slam_toolbox/src/slam_toolbox_async.cpp b/slam_toolbox/src/slam_toolbox_async.cpp index 87de8347..96cdbb12 100644 --- a/slam_toolbox/src/slam_toolbox_async.cpp +++ b/slam_toolbox/src/slam_toolbox_async.cpp @@ -32,16 +32,27 @@ 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 + // no odom info on any pose helper karto::Pose2 pose; - if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) - { + if(pose_helpers_.find(base_frame_id) == pose_helpers_.end()) return; + else + { + pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id); } + // 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 a2f461c8..36675a90 100644 --- a/slam_toolbox/src/slam_toolbox_common.cpp +++ b/slam_toolbox/src/slam_toolbox_common.cpp @@ -40,10 +40,22 @@ SlamToolbox::SlamToolbox(ros::NodeHandle& nh) setROSInterfaces(nh_); setSolver(nh_); - laser_assistant_ = std::make_unique( - nh_, tf_.get(), base_frame_); - pose_helper_ = std::make_unique( - tf_.get(), base_frame_, odom_frame_); + if(base_frames_.size() != laser_topics_.size()) + ROS_FATAL("[RoboSAR:slam_toolbox_common:SlamToolbox] base_frames_.size() != laser_topics_.size()"); + if(base_frames_.size() != odom_frames_.size()) + 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()); + // 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 for each robot + for(size_t idx = 0; idx < base_frames_.size(); idx++) + { + pose_helpers_[base_frames_[idx]] = std::make_unique(tf_.get(), base_frames_[idx], odom_frames_[idx]); + } scan_holder_ = std::make_unique(lasers_); map_saver_ = std::make_unique(nh_, map_name_); closure_assistant_ = @@ -74,8 +86,14 @@ SlamToolbox::~SlamToolbox() dataset_.reset(); closure_assistant_.reset(); map_saver_.reset(); - pose_helper_.reset(); - laser_assistant_.reset(); + for(std::map>::iterator it = pose_helpers_.begin(); it != pose_helpers_.end(); it++) + { + it->second.reset(); + } + for(std::map>::iterator it = laser_assistants_.begin(); it != laser_assistants_.end(); it++) + { + it->second.reset(); + } scan_holder_.reset(); } @@ -108,13 +126,24 @@ void SlamToolbox::setSolver(ros::NodeHandle& private_nh_) void SlamToolbox::setParams(ros::NodeHandle& private_nh) /*****************************************************************************/ { - map_to_odom_.setIdentity(); - private_nh.param("odom_frame", odom_frame_, std::string("odom")); private_nh.param("map_frame", map_frame_, std::string("map")); - private_nh.param("base_frame", base_frame_, std::string("base_footprint")); private_nh.param("resolution", resolution_, 0.05); private_nh.param("map_name", map_name_, std::string("/map")); - private_nh.param("scan_topic", scan_topic_, std::string("/scan")); + std::vector default_laser = {"/scan"}; + if (!private_nh.getParam("laser_topics", laser_topics_)) + { + laser_topics_ = default_laser; + } + std::vector default_base_frame = {"base_footprint"}; + if (!private_nh.getParam("base_frames", base_frames_)) + { + base_frames_ = default_base_frame; + } + std::vector default_odom_frame = {"odom"}; + if (!private_nh.getParam("odom_frames", odom_frames_)) + { + odom_frames_ = default_odom_frame; + } private_nh.param("throttle_scans", throttle_scans_, 1); private_nh.param("enable_interactive_mode", enable_interactive_mode_, false); @@ -153,9 +182,13 @@ void SlamToolbox::setROSInterfaces(ros::NodeHandle& node) ssPauseMeasurements_ = node.advertiseService("pause_new_measurements", &SlamToolbox::pauseNewMeasurementsCallback, this); ssSerialize_ = node.advertiseService("serialize_map", &SlamToolbox::serializePoseGraphCallback, this); ssDesserialize_ = node.advertiseService("deserialize_map", &SlamToolbox::deserializePoseGraphCallback, this); - scan_filter_sub_ = std::make_unique >(node, scan_topic_, 5); - scan_filter_ = std::make_unique >(*scan_filter_sub_, *tf_, odom_frame_, 5, node); - scan_filter_->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1)); + for(size_t idx = 0; idx < laser_topics_.size(); idx++) + { + 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, base_frames_[idx])); + } } /*****************************************************************************/ @@ -171,13 +204,23 @@ void SlamToolbox::publishTransformLoop(const double& transform_publish_period) while(ros::ok()) { { - boost::mutex::scoped_lock lock(map_to_odom_mutex_); - geometry_msgs::TransformStamped msg; - tf2::convert(map_to_odom_, msg.transform); - msg.child_frame_id = odom_frame_; - msg.header.frame_id = map_frame_; - msg.header.stamp = ros::Time::now() + transform_timeout_; - tfB_->sendTransform(msg); + // Create copy of map-to-odom TFs map + std::map local_TFs_map; + { + boost::mutex::scoped_lock lock(map_to_odom_mutex_); + local_TFs_map = m_map_to_odoms_; + } + // Publish all past and current transforms so none of them go stale + std::map::const_iterator iter; + for(iter = local_TFs_map.begin(); iter != local_TFs_map.end(); iter++) + { + geometry_msgs::TransformStamped msg; + tf2::convert(iter->second, msg.transform); + msg.child_frame_id = iter->first; + msg.header.frame_id = map_frame_; + msg.header.stamp = ros::Time::now() + transform_timeout_; + tfB_->sendTransform(msg); + } } r.sleep(); } @@ -286,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_assistant_->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) @@ -336,22 +386,36 @@ bool SlamToolbox::updateMap() tf2::Stamped SlamToolbox::setTransformFromPoses( const karto::Pose2& corrected_pose, const karto::Pose2& karto_pose, - const ros::Time& t, + const std_msgs::Header& header, const bool& update_reprocessing_transform) /*****************************************************************************/ { - // Compute the map->odom transform 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::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, base_frame_); + corrected_pose.GetY(), 0.0)).inverse(), t, base_frame); try { geometry_msgs::TransformStamped base_to_map_msg, odom_to_map_msg; tf2::convert(base_to_map, base_to_map_msg); - odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame_); + odom_to_map_msg = tf_->transform(base_to_map_msg, odom_frame); tf2::convert(odom_to_map_msg, odom_to_map); } catch(tf2::TransformException& e) @@ -376,9 +440,11 @@ tf2::Stamped SlamToolbox::setTransformFromPoses( } // set map to odom for our transformation thread to publish - boost::mutex::scoped_lock lock(map_to_odom_mutex_); - map_to_odom_ = tf2::Transform(tf2::Quaternion( odom_to_map.getRotation() ), + tf2::Transform map_to_odom_tf = tf2::Transform( + tf2::Quaternion( odom_to_map.getRotation() ), tf2::Vector3( odom_to_map.getOrigin() ) ).inverse(); + boost::mutex::scoped_lock lock(map_to_odom_mutex_); + m_map_to_odoms_[odom_frame] = map_to_odom_tf; return odom_to_map; } @@ -413,17 +479,26 @@ bool SlamToolbox::shouldProcessScan( const karto::Pose2& pose) /*****************************************************************************/ { - static karto::Pose2 last_pose; - static ros::Time last_scan_time = ros::Time(0.); + static std::vector scan_frame_ids; + static std::map last_poses; + static std::map last_scan_times; static double min_dist2 = smapper_->getMapper()->getParamMinimumTravelDistance() * smapper_->getMapper()->getParamMinimumTravelDistance(); - + // Check if frame_id of current scan is new + bool new_scan_frame_id = false; + std::string cur_frame_id = scan->header.frame_id; + if (std::find(scan_frame_ids.begin(), scan_frame_ids.end(), cur_frame_id) == scan_frame_ids.end()){ + // New scan + new_scan_frame_id = true; + scan_frame_ids.push_back(cur_frame_id); + last_scan_times[cur_frame_id] = ros::Time(0.); + } // we give it a pass on the first measurement to get the ball rolling - if (first_measurement_) + if (first_measurement_ || new_scan_frame_id) { - last_scan_time = scan->header.stamp; - last_pose = pose; + last_scan_times[cur_frame_id] = scan->header.stamp; + last_poses[cur_frame_id] = pose; first_measurement_ = false; return true; } @@ -441,20 +516,20 @@ bool SlamToolbox::shouldProcessScan( } // not enough time - if (scan->header.stamp - last_scan_time < minimum_time_interval_) + if (scan->header.stamp - last_scan_times[cur_frame_id] < minimum_time_interval_) { return false; } // check moved enough, within 10% for correction error - const double dist2 = last_pose.SquaredDistance(pose); + const double dist2 = last_poses[cur_frame_id].SquaredDistance(pose); if(dist2 < 0.8 * min_dist2 || scan->header.seq < 5) { return false; } - last_pose = pose; - last_scan_time = scan->header.stamp; + last_poses[cur_frame_id] = pose; + last_scan_times[cur_frame_id] = scan->header.stamp; return true; } @@ -525,7 +600,7 @@ karto::LocalizedRangeScan* SlamToolbox::addScan( } setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, - scan->header.stamp, update_reprocessing_transform); + scan->header, update_reprocessing_transform); dataset_->Add(range_scan); } else @@ -670,14 +745,20 @@ void SlamToolbox::loadSerializedPoseGraph( ROS_INFO("Waiting for incoming scan to get metadata..."); boost::shared_ptr scan = ros::topic::waitForMessage( - scan_topic_, ros::Duration(1.0)); + laser_topics_.front(), ros::Duration(1.0)); if (scan) { ROS_INFO("Got scan!"); try { - lasers_[scan->header.frame_id] = - laser_assistant_->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 fc169b5c..baafed83 100644 --- a/slam_toolbox/src/slam_toolbox_localization.cpp +++ b/slam_toolbox/src/slam_toolbox_localization.cpp @@ -101,14 +101,16 @@ 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 - Pose2 pose; - if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) - { +// no odom info on any pose helper + karto::Pose2 pose; + if(pose_helpers_.find(base_frame_id) == pose_helpers_.end()) return; + else + { + pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id); } // ensure the laser can be used @@ -188,7 +190,7 @@ LocalizedRangeScan* LocalizationSlamToolbox::addScan( } else { // compute our new transform setTransformFromPoses(range_scan->GetCorrectedPose(), karto_pose, - scan->header.stamp, update_reprocessing_transform); + scan->header, update_reprocessing_transform); } return range_scan; diff --git a/slam_toolbox/src/slam_toolbox_sync.cpp b/slam_toolbox/src/slam_toolbox_sync.cpp index a923f94b..216e50cb 100644 --- a/slam_toolbox/src/slam_toolbox_sync.cpp +++ b/slam_toolbox/src/slam_toolbox_sync.cpp @@ -65,16 +65,27 @@ 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 + // no odom info on any pose helper karto::Pose2 pose; - if(!pose_helper_->getOdomPose(pose, scan->header.stamp)) - { + if(pose_helpers_.find(base_frame_id) == pose_helpers_.end()) return; + else + { + pose_helpers_[base_frame_id]->getOdomPose(pose, scan->header.stamp, base_frame_id); } + // 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);