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.

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-robot SLAM #541

Open
wants to merge 20 commits into
base: ros1_multirobot
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
35b9322
Made makeLaser use frame_id from scan
jsongCMU Oct 16, 2022
4684f95
Allowed subscription to multiple laser scans
jsongCMU Oct 16, 2022
c6a66fb
Allowed multirobot odom tf retrieval; updated yaml for multiple odom …
jsongCMU Oct 16, 2022
58cb61b
Made each robot get its own laser assistant
jsongCMU Oct 16, 2022
7157b98
Added error messages for different number of frames/topics
jsongCMU Oct 16, 2022
9927c9f
Changed setTransformFromPoses to take in header instead of just stamp
jsongCMU Oct 16, 2022
d424c89
Changed map-odom tf to use laser frame from scan header, and odom fra…
jsongCMU Oct 16, 2022
06a6498
Publish all map->base_link tfs to prevent them from going stale
jsongCMU Oct 16, 2022
212369e
Fixed incorrect constraints between first scans
jsongCMU Sep 27, 2022
9202adb
Made shouldProcessScan handle multiple robots with starting odoms of …
jsongCMU Sep 29, 2022
d98d737
Remove default arg for ref_frame
jsongCMU Dec 19, 2022
154e0ea
Use base frame to lookup odom frame
jsongCMU Dec 21, 2022
aef6cc4
Make scan topics global scoped
jsongCMU Dec 21, 2022
a358c6d
setTransformFromPoses updates m_map_to_odoms_ directly
jsongCMU Dec 22, 2022
ebee42b
Reduce mutex scope in publishTransformLoop
jsongCMU Dec 22, 2022
0f76690
Laser frame no longer needs to be same as base frame (#2)
jsongCMU Jan 2, 2023
f5d36ea
Make edge and node color robot dependent in RVIZ
jsongCMU Jan 2, 2023
99936a3
Changed pose_helpers_ from vector to map
jsongCMU Feb 5, 2023
6c950a7
Added MRSLAM gif showing mapping and loop closure
jsongCMU Feb 5, 2023
40d0bff
Updated async yaml file
jsongCMU Feb 5, 2023
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
6 changes: 3 additions & 3 deletions slam_toolbox/config/mapper_params_online_sync.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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"]
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Make these changes to all the yaml files so that they all work in this branch with the change of types

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I updated the yaml for sync and async; I'm not currently sure what would be the best way to handle localization and life-long, since they're very different algorithms; what're your thoughts on this?

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
Expand Down
9 changes: 8 additions & 1 deletion slam_toolbox/include/slam_toolbox/get_pose_helper.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,15 @@ 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 = std::string(""))
jsongCMU marked this conversation as resolved.
Show resolved Hide resolved
{
// 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_;
Expand Down
16 changes: 10 additions & 6 deletions slam_toolbox/include/slam_toolbox/slam_toolbox_common.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@
#include <fstream>
#include <boost/thread.hpp>
#include <sys/resource.h>
#include <assert.h>

namespace slam_toolbox
{
Expand Down Expand Up @@ -86,7 +87,7 @@ class SlamToolbox
karto::LocalizedRangeScan* addScan(karto::LaserRangeFinder* laser, PosedScan& scanWPose);
bool updateMap();
tf2::Stamped<tf2::Transform> 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);
Expand All @@ -103,13 +104,14 @@ class SlamToolbox
std::unique_ptr<tf2_ros::Buffer> tf_;
std::unique_ptr<tf2_ros::TransformListener> tfL_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tfB_;
std::unique_ptr<message_filters::Subscriber<sensor_msgs::LaserScan> > scan_filter_sub_;
std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > scan_filter_;
std::vector<std::unique_ptr<message_filters::Subscriber<sensor_msgs::LaserScan> > > scan_filter_subs_;
std::vector<std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > > 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<std::string> odom_frames_, base_frames_, laser_topics_;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
ros::Duration transform_timeout_, tf_buffer_dur_, minimum_time_interval_;
int throttle_scans_;

Expand All @@ -120,17 +122,19 @@ class SlamToolbox
std::unique_ptr<mapper_utils::SMapper> smapper_;
std::unique_ptr<karto::Dataset> dataset_;
std::map<std::string, laser_utils::LaserMetadata> lasers_;
std::map<std::string, tf2::Transform> m_map_to_odoms_;

// helpers
std::unique_ptr<laser_utils::LaserAssistant> laser_assistant_;
std::unique_ptr<pose_utils::GetPoseHelper> pose_helper_;
std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>> laser_assistants_;
std::vector<std::unique_ptr<pose_utils::GetPoseHelper>> pose_helpers_;
std::unique_ptr<map_saver::MapSaver> map_saver_;
std::unique_ptr<loop_closure_assistant::LoopClosureAssistant> closure_assistant_;
std::unique_ptr<laser_utils::ScanHolder> scan_holder_;

// Internal state
std::vector<std::unique_ptr<boost::thread> > threads_;
tf2::Transform map_to_odom_;
std::string map_to_odom_child_frame_id_;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
boost::mutex map_to_odom_mutex_, smapper_mutex_, pose_mutex_;
PausedState state_;
nav_msgs::GetMap::Response map_;
Expand Down
2 changes: 1 addition & 1 deletion slam_toolbox/lib/karto_sdk/src/Mapper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1513,7 +1513,7 @@ namespace karto
kt_double response = m_pMapper->m_pSequentialScanMatcher->MatchScan<LocalizedRangeScanMap>(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())
Expand Down
13 changes: 9 additions & 4 deletions slam_toolbox/src/experimental/slam_toolbox_lifelong.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,12 +69,17 @@ void LifelongSlamToolbox::laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan)
/*****************************************************************************/
{
// no odom info
Pose2 pose;
if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
// no odom info on any pose helper
karto::Pose2 pose;
bool found_odom = false;
for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
{
return;
found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
if(found_odom)
break;
}
if(!found_odom)
return;

// ensure the laser can be used
LaserRangeFinder* laser = getLaser(scan);
Expand Down
3 changes: 2 additions & 1 deletion slam_toolbox/src/laser_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
11 changes: 8 additions & 3 deletions slam_toolbox/src/slam_toolbox_async.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,17 @@ void AsynchronousSlamToolbox::laserCallback(
const sensor_msgs::LaserScan::ConstPtr& scan)
/*****************************************************************************/
{
// no odom info
// no odom info on any pose helper
karto::Pose2 pose;
if(!pose_helper_->getOdomPose(pose, scan->header.stamp))
bool found_odom = false;
for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
{
return;
found_odom = pose_helpers_[idx]->getOdomPose(pose, scan->header.stamp, scan->header.frame_id);
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
if(found_odom)
break;
}
if(!found_odom)
return;

// ensure the laser can be used
karto::LaserRangeFinder* laser = getLaser(scan);
Expand Down
123 changes: 88 additions & 35 deletions slam_toolbox/src/slam_toolbox_common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,18 @@ SlamToolbox::SlamToolbox(ros::NodeHandle& nh)
setROSInterfaces(nh_);
setSolver(nh_);

laser_assistant_ = std::make_unique<laser_utils::LaserAssistant>(
nh_, tf_.get(), base_frame_);
pose_helper_ = std::make_unique<pose_utils::GetPoseHelper>(
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());

for(size_t idx = 0; idx < base_frames_.size(); idx++)
{
pose_helpers_.push_back(std::make_unique<pose_utils::GetPoseHelper>(tf_.get(), base_frames_[idx], odom_frames_[idx]));
jsongCMU marked this conversation as resolved.
Show resolved Hide resolved
laser_assistants_[base_frames_[idx]] = std::make_unique<laser_utils::LaserAssistant>(nh_, tf_.get(), base_frames_[idx]); // Assumes base frame = laser frame
}
scan_holder_ = std::make_unique<laser_utils::ScanHolder>(lasers_);
map_saver_ = std::make_unique<map_saver::MapSaver>(nh_, map_name_);
closure_assistant_ =
Expand Down Expand Up @@ -74,8 +82,15 @@ SlamToolbox::~SlamToolbox()
dataset_.reset();
closure_assistant_.reset();
map_saver_.reset();
pose_helper_.reset();
laser_assistant_.reset();
for(size_t idx = 0; idx < pose_helpers_.size(); idx++)
{
pose_helpers_[idx].reset();
// laser_assistants_.reset();
}
for(std::map<std::string,std::unique_ptr<laser_utils::LaserAssistant>>::iterator it = laser_assistants_.begin(); it != laser_assistants_.end(); it++)
{
it->second.reset();
}
scan_holder_.reset();
}

Expand Down Expand Up @@ -109,12 +124,24 @@ 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<std::string> default_laser = {"/scan"};
if (!private_nh.getParam("laser_topics", laser_topics_))
{
laser_topics_ = default_laser;
}
std::vector<std::string> default_base_frame = {"base_footprint"};
if (!private_nh.getParam("base_frames", base_frames_))
{
base_frames_ = default_base_frame;
}
std::vector<std::string> 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);

Expand Down Expand Up @@ -153,9 +180,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<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, scan_topic_, 5);
scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*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<message_filters::Subscriber<sensor_msgs::LaserScan> >(node, laser_topics_[idx], 5));
scan_filters_.push_back(std::make_unique<tf2_ros::MessageFilter<sensor_msgs::LaserScan> >(*scan_filter_subs_.back(), *tf_, odom_frames_[idx], 5, node));
scan_filters_.back()->registerCallback(boost::bind(&SlamToolbox::laserCallback, this, _1));
}
}

/*****************************************************************************/
Expand All @@ -172,12 +203,20 @@ void SlamToolbox::publishTransformLoop(const double& transform_publish_period)
{
{
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);
// Update with / add latest transform
if(map_to_odom_child_frame_id_.length() > 0)
m_map_to_odoms_[map_to_odom_child_frame_id_] = map_to_odom_;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
// Publish all past and current transforms so none of them go stale
std::map<std::string, tf2::Transform>::iterator iter;
for(iter = m_map_to_odoms_.begin(); iter != m_map_to_odoms_.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();
}
Expand Down Expand Up @@ -291,7 +330,7 @@ karto::LaserRangeFinder* SlamToolbox::getLaser(const
{
try
{
lasers_[frame] = laser_assistant_->toLaserMetadata(*scan);
lasers_[frame] = laser_assistants_[frame]->toLaserMetadata(*scan);
dataset_->Add(lasers_[frame].getLaser(), true);
}
catch (tf2::TransformException& e)
Expand Down Expand Up @@ -336,22 +375,26 @@ bool SlamToolbox::updateMap()
tf2::Stamped<tf2::Transform> 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)
/*****************************************************************************/
{
// Turn base frame into odom frame
std::string robot_name = header.frame_id.substr(0, header.frame_id.find("/"));
std::string odom_frame = robot_name + "/" + "odom"; // TODO: Make not hard coded
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
// Compute the map->odom transform
const ros::Time& t = header.stamp;
tf2::Stamped<tf2::Transform> odom_to_map;
tf2::Quaternion q(0.,0.,0.,1.0);
q.setRPY(0., 0., corrected_pose.GetHeading());
tf2::Stamped<tf2::Transform> 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, header.frame_id); // Assumes base frame = laser 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)
Expand Down Expand Up @@ -379,6 +422,7 @@ tf2::Stamped<tf2::Transform> SlamToolbox::setTransformFromPoses(
boost::mutex::scoped_lock lock(map_to_odom_mutex_);
map_to_odom_ = tf2::Transform(tf2::Quaternion( odom_to_map.getRotation() ),
tf2::Vector3( odom_to_map.getOrigin() ) ).inverse();
map_to_odom_child_frame_id_ = odom_frame;

return odom_to_map;
}
Expand Down Expand Up @@ -413,17 +457,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<std::string> scan_frame_ids;
static std::map<std::string, karto::Pose2> last_poses;
static std::map<std::string, ros::Time> 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;
}
Expand All @@ -441,20 +494,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;
}
Expand Down Expand Up @@ -525,7 +578,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
Expand Down Expand Up @@ -670,14 +723,14 @@ void SlamToolbox::loadSerializedPoseGraph(
ROS_INFO("Waiting for incoming scan to get metadata...");
boost::shared_ptr<sensor_msgs::LaserScan const> scan =
ros::topic::waitForMessage<sensor_msgs::LaserScan>(
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);
laser_assistants_[scan->header.frame_id]->toLaserMetadata(*scan);
break;
}
catch (tf2::TransformException& e)
Expand Down
Loading